diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 6a987dc269e76..cfb945270ed7f 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -45,7 +45,7 @@ namespace motion_utils * @param input_path input path(point) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -53,7 +53,7 @@ namespace motion_utils */ std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -63,7 +63,7 @@ std::vector resamplePointVector( * based on the interpolated position x and y. * @param input_path input path(position) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -71,7 +71,7 @@ std::vector resamplePointVector( */ std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path(poses). Note that in a default setting, position xy are @@ -81,7 +81,7 @@ std::vector resamplePointVector( * @param input_path input path(poses) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -89,7 +89,7 @@ std::vector resamplePointVector( */ std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -99,7 +99,7 @@ std::vector resamplePoseVector( * based on the interpolated position x and y. * @param input_path input path(poses) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -107,7 +107,7 @@ std::vector resamplePoseVector( */ std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path with lane id. Note that in a default setting, position xy @@ -119,7 +119,7 @@ std::vector resamplePoseVector( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -129,7 +129,7 @@ std::vector resamplePoseVector( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -141,7 +141,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * and is_final are also interpolated by zero order hold * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -152,7 +152,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); @@ -165,7 +165,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -175,7 +175,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -186,7 +186,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * forward difference method based on the interpolated position x and y. * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -197,7 +197,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** @@ -210,7 +210,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param input_trajectory input trajectory to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -220,7 +220,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); /** @@ -233,7 +233,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * method based on the interpolated position x and y. * @param input_trajectory input trajectory to resample * @param resampled_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -245,7 +245,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 5417da86f5359..14f44b7892645 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -21,7 +21,7 @@ namespace motion_utils { std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -60,9 +60,12 @@ std::vector resamplePointVector( const auto spline = [&](const auto & input) { return interpolation::spline(input_arclength, input, resampled_arclength); }; + const auto spline_by_akima = [&](const auto & input) { + return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); + const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; @@ -82,7 +85,7 @@ std::vector resamplePointVector( std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -102,12 +105,12 @@ std::vector resamplePointVector( resampling_arclength.push_back(input_length); } - return resamplePointVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -120,7 +123,7 @@ std::vector resamplePoseVector( position.at(i) = points.at(i).position; } const auto resampled_position = - resamplePointVector(position, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); std::vector resampled_points(resampled_position.size()); @@ -148,7 +151,7 @@ std::vector resamplePoseVector( std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -168,12 +171,12 @@ std::vector resamplePoseVector( resampling_arclength.push_back(input_length); } - return resamplePoseVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -245,7 +248,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -279,7 +282,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -339,12 +342,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, use_zero_order_hold_for_v); + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_v); } autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -390,7 +394,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -420,8 +424,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, - const bool resample_input_path_stop_point) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { // validate arguments if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { @@ -473,13 +477,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); } autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { // validate arguments @@ -542,7 +546,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -579,7 +583,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments @@ -632,7 +636,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( } return resampleTrajectory( - input_trajectory, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); }