feat(interpolation): add option to enable akima spline for xy (autowa…

…refoundation#2802)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/motion_utils/include/motion_utils/resample/resample.hpp

@@ -45,15 +45,15 @@ 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
  * @return resampled path(poses)
  */
 std::vector<geometry_msgs::msg::Point> resamplePointVector(
   const std::vector<geometry_msgs::msg::Point> & points,
-  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
   const bool use_lerp_for_z = true);
 
 /**
@@ -63,15 +63,15 @@ std::vector<geometry_msgs::msg::Point> 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
  * @return resampled path(poses)
  */
 std::vector<geometry_msgs::msg::Point> resamplePointVector(
   const std::vector<geometry_msgs::msg::Point> & 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,15 +81,15 @@ std::vector<geometry_msgs::msg::Point> 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
  * @return resampled path(poses)
  */
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
   const std::vector<geometry_msgs::msg::Pose> & points,
-  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
   const bool use_lerp_for_z = true);
 
 /**
@@ -99,15 +99,15 @@ std::vector<geometry_msgs::msg::Pose> 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
  * @return resampled path(poses)
  */
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
   const std::vector<geometry_msgs::msg::Pose> & 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<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose> resamplePoseVector(
  */
 autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
-  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const std::vector<double> & 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<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const std::vector<double> & 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<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const std::vector<double> & 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

common/motion_utils/src/resample/resample.cpp

@@ -21,7 +21,7 @@ namespace motion_utils
 {
 std::vector<geometry_msgs::msg::Point> resamplePointVector(
   const std::vector<geometry_msgs::msg::Point> & points,
-  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
   // validate arguments
@@ -60,9 +60,12 @@ std::vector<geometry_msgs::msg::Point> 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<geometry_msgs::msg::Point> resampled_points;
@@ -82,7 +85,7 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
 
 std::vector<geometry_msgs::msg::Point> resamplePointVector(
   const std::vector<geometry_msgs::msg::Point> & 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<geometry_msgs::msg::Point> 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<geometry_msgs::msg::Pose> resamplePoseVector(
   const std::vector<geometry_msgs::msg::Pose> & points,
-  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
   // validate arguments
@@ -120,7 +123,7 @@ std::vector<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose> resampled_points(resampled_position.size());
 
@@ -148,7 +151,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
 
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
   const std::vector<geometry_msgs::msg::Pose> & 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<geometry_msgs::msg::Pose> 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<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const std::vector<double> & 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<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const std::vector<double> & 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<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const std::vector<double> & 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);
 }