diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index e1a60c86f036a..d44d16d88dd04 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -21,6 +21,10 @@ namespace interpolation { +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys); + template std::vector splineYawFromPoints(const std::vector & points); } // namespace interpolation diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 2570e01790487..71e2629044f11 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -68,6 +68,29 @@ std::array, 3> getBaseValues( namespace interpolation { + +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys) +{ + SplineInterpolation interpolator_x, interpolator_y; + std::vector yaw_vec; + + // calculate spline coefficients + interpolator_x.calcSplineCoefficients(base_keys, base_x_values); + interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); + const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + for (size_t i = 0; i < diff_x.size(); i++) { + double yaw = std::atan2(diff_y[i], diff_x[i]); + yaw_vec.push_back(yaw); + } + // interpolate base_keys at query_keys + return { + interpolator_x.getSplineInterpolatedValues(query_keys), + interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec}; +} + template std::vector splineYawFromPoints(const std::vector & points) { @@ -86,6 +109,7 @@ std::vector splineYawFromPoints(const std::vector & points) } template std::vector splineYawFromPoints( const std::vector & points); + } // namespace interpolation geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index a3b229ba55276..b21254ef20297 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -18,6 +18,7 @@ #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" @@ -115,6 +116,9 @@ std::vector interpolate2DTraj const std::vector & base_x, const std::vector & base_y, const std::vector & base_yaw, const double resolution); +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution); + template std::vector getInterpolatedPoints( const T & points, const double delta_arc_length, const double offset = 0) diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index c45d7c808586d..f5f030dcd4ac8 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -366,13 +366,55 @@ std::vector interpolate2DTraj return interpolated_points; } +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution) +{ + if (base_x.empty() || base_y.empty()) { + return std::vector{}; + } + std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + std::vector new_s; + for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + + // spline interpolation + // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] + std::array, 3> interpolated = + interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); + const auto & interpolated_x = interpolated[0]; + const auto & interpolated_y = interpolated[1]; + const auto & interpolated_yaw = interpolated[2]; + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + + return interpolated_points; +} + std::vector getInterpolatedTrajectoryPoints( const std::vector & points, const double delta_arc_length) { std::array, 3> validated_pose = validateTrajectoryPoints(points); return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), validated_pose.at(2), delta_arc_length); + validated_pose.at(0), validated_pose.at(1), delta_arc_length); } std::vector getConnectedInterpolatedPoints(