fix(obstacle_avoidance_planner, interpolation): improve slerp and fix…

… yaw (#1687)

* merge slerp of x y yaw

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

* ci(pre-commit): autofix

* move and rename new slerp

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

* add include file

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp

@@ -21,6 +21,10 @@
 
 namespace interpolation
 {
+std::array<std::vector<double>, 3> slerp2dFromXY(
+  const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
+  const std::vector<double> & base_y_values, const std::vector<double> & query_keys);
+
 template <typename T>
 std::vector<double> splineYawFromPoints(const std::vector<T> & points);
 }  // namespace interpolation

common/interpolation/src/spline_interpolation_points_2d.cpp

@@ -68,6 +68,29 @@ std::array<std::vector<double>, 3> getBaseValues(
 
 namespace interpolation
 {
+
+std::array<std::vector<double>, 3> slerp2dFromXY(
+  const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
+  const std::vector<double> & base_y_values, const std::vector<double> & query_keys)
+{
+  SplineInterpolation interpolator_x, interpolator_y;
+  std::vector<double> 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 <typename T>
 std::vector<double> splineYawFromPoints(const std::vector<T> & points)
 {
@@ -86,6 +109,7 @@ std::vector<double> splineYawFromPoints(const std::vector<T> & points)
 }
 template std::vector<double> splineYawFromPoints(
   const std::vector<geometry_msgs::msg::Point> & points);
+
 }  // namespace interpolation
 
 geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(

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<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   const std::vector<double> & base_x, const std::vector<double> & base_y,
   const std::vector<double> & base_yaw, const double resolution);
 
+std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTrajectoryPoints(
+  const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution);
+
 template <typename T>
 std::vector<geometry_msgs::msg::Point> getInterpolatedPoints(
   const T & points, const double delta_arc_length, const double offset = 0)

planning/obstacle_avoidance_planner/src/utils.cpp

@@ -366,13 +366,55 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   return interpolated_points;
 }
 
+std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTrajectoryPoints(
+  const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution)
+{
+  if (base_x.empty() || base_y.empty()) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+  }
+  std::vector<double> base_s = calcEuclidDist(base_x, base_y);
+  if (base_s.empty() || base_s.size() == 1) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+  }
+  std::vector<double> 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<std::vector<double>, 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<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+    }
+  }
+
+  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> 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<autoware_auto_planning_msgs::msg::TrajectoryPoint> getInterpolatedTrajectoryPoints(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
   const double delta_arc_length)
 {
   std::array<std::vector<double>, 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<autoware_auto_planning_msgs::msg::TrajectoryPoint> getConnectedInterpolatedPoints(