refactor(behavior_velocity)!: unite spline interpolation

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -33,10 +33,6 @@ namespace util
 bool setVelocityFrom(
   const size_t idx, const double vel, autoware_auto_planning_msgs::msg::PathWithLaneId * input);
 
-bool splineInterpolate(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger);
-
 int insertPoint(
   const geometry_msgs::msg::Pose & in_pose,
   autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);

planning/behavior_velocity_planner/include/utilization/interpolate.hpp

@@ -15,15 +15,21 @@
 #ifndef UTILIZATION__INTERPOLATE_HPP_
 #define UTILIZATION__INTERPOLATE_HPP_
 
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
 #include <cmath>
 #include <vector>
 
 namespace behavior_velocity_planner
 {
 namespace interpolation
 {
-// template <class T>
-// bool splineInterpolateWithFixInterval(const T & input, const double interval, T * output);
+bool splineInterpolate(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger);
 
 class LinearInterpolate
 {

planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp

@@ -200,7 +200,7 @@ bool BlindSpotModule::generateStopLine(
 
   /* spline interpolation */
   autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!util::splineInterpolate(*path, interval, &path_ip, logger_)) {
+  if (!interpolation::splineInterpolate(*path, interval, &path_ip, logger_)) {
     return false;
   }
   debug_data_.spline_path = path_ip;

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -19,6 +19,7 @@
 #include <scene_module/intersection/util.hpp>
 #include <utilization/boost_geometry_helper.hpp>
 #include <utilization/interpolate.hpp>
+#include <utilization/path_utilization.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -60,82 +60,6 @@ int insertPoint(
   return insert_idx;
 }
 
-bool splineInterpolate(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
-{
-  *output = input;
-
-  if (input.points.size() <= 1) {
-    RCLCPP_WARN(logger, "Do not interpolate because path size is 1.");
-    return false;
-  }
-
-  static constexpr double ep = 1.0e-8;
-
-  // calc arclength for path
-  std::vector<double> base_x;
-  std::vector<double> base_y;
-  std::vector<double> base_z;
-  for (const auto & p : input.points) {
-    base_x.push_back(p.point.pose.position.x);
-    base_y.push_back(p.point.pose.position.y);
-    base_z.push_back(p.point.pose.position.z);
-  }
-  std::vector<double> base_s = interpolation::calcEuclidDist(base_x, base_y);
-
-  // remove duplicating sample points
-  {
-    size_t Ns = base_s.size();
-    size_t i = 1;
-    while (i < Ns) {
-      if (std::fabs(base_s[i - 1] - base_s[i]) < ep) {
-        base_s.erase(base_s.begin() + i);
-        base_x.erase(base_x.begin() + i);
-        base_y.erase(base_y.begin() + i);
-        base_z.erase(base_z.begin() + i);
-        Ns -= 1;
-        i -= 1;
-      }
-      ++i;
-    }
-  }
-
-  std::vector<double> resampled_s;
-  for (double d = 0.0; d < base_s.back() - ep; d += interval) {
-    resampled_s.push_back(d);
-  }
-
-  // do spline for xy
-  const std::vector<double> resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s);
-  const std::vector<double> resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s);
-  const std::vector<double> resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s);
-
-  // set xy
-  output->points.clear();
-  for (size_t i = 0; i < resampled_s.size(); i++) {
-    autoware_auto_planning_msgs::msg::PathPointWithLaneId p;
-    p.point.pose.position.x = resampled_x.at(i);
-    p.point.pose.position.y = resampled_y.at(i);
-    p.point.pose.position.z = resampled_z.at(i);
-    output->points.push_back(p);
-  }
-
-  // set yaw
-  for (int i = 1; i < static_cast<int>(resampled_s.size()) - 1; i++) {
-    auto p = output->points.at(i - 1).point.pose.position;
-    auto n = output->points.at(i + 1).point.pose.position;
-    double yaw = std::atan2(n.y - p.y, n.x - p.x);
-    output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
-  }
-  if (output->points.size() > 1) {
-    size_t l = output->points.size();
-    output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
-  }
-  return true;
-}
-
 geometry_msgs::msg::Pose getAheadPose(
   const size_t start_idx, const double ahead_dist,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
@@ -261,7 +185,7 @@ bool generateStopLine(
 
   /* spline interpolation */
   autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!util::splineInterpolate(target_path, interval, &path_ip, logger)) {
+  if (!interpolation::splineInterpolate(target_path, interval, &path_ip, logger)) {
     return false;
   }
 

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

@@ -16,6 +16,7 @@
 
 #include "utilization/arc_lane_util.hpp"
 #include "utilization/interpolate.hpp"
+#include "utilization/path_utilization.hpp"
 #include "utilization/util.hpp"
 
 #include <interpolation/spline_interpolation.hpp>
@@ -33,81 +34,6 @@
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
-bool splineInterpolate(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
-{
-  *output = input;
-
-  if (input.points.size() <= 1) {
-    RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
-    return false;
-  }
-
-  static constexpr double ep = 1.0e-8;
-
-  // calc arclength for path
-  std::vector<double> base_x;
-  std::vector<double> base_y;
-  std::vector<double> base_z;
-  for (const auto & p : input.points) {
-    base_x.push_back(p.point.pose.position.x);
-    base_y.push_back(p.point.pose.position.y);
-    base_z.push_back(p.point.pose.position.z);
-  }
-  std::vector<double> base_s = interpolation::calcEuclidDist(base_x, base_y);
-
-  // remove duplicating sample points
-  {
-    size_t Ns = base_s.size();
-    size_t i = 1;
-    while (i < Ns) {
-      if (std::fabs(base_s[i - 1] - base_s[i]) < ep) {
-        base_s.erase(base_s.begin() + i);
-        base_x.erase(base_x.begin() + i);
-        base_y.erase(base_y.begin() + i);
-        base_z.erase(base_z.begin() + i);
-        Ns -= 1;
-        i -= 1;
-      }
-      ++i;
-    }
-  }
-
-  std::vector<double> resampled_s;
-  for (double d = 0.0; d < base_s.back() - ep; d += interval) {
-    resampled_s.push_back(d);
-  }
-
-  // do spline for xy
-  const std::vector<double> resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s);
-  const std::vector<double> resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s);
-  const std::vector<double> resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s);
-
-  // set xy
-  output->points.clear();
-  for (size_t i = 0; i < resampled_s.size(); i++) {
-    autoware_auto_planning_msgs::msg::PathPointWithLaneId p;
-    p.point.pose.position.x = resampled_x.at(i);
-    p.point.pose.position.y = resampled_y.at(i);
-    p.point.pose.position.z = resampled_z.at(i);
-    output->points.push_back(p);
-  }
-
-  // set yaw
-  for (int i = 1; i < static_cast<int>(resampled_s.size()) - 1; i++) {
-    auto p = output->points.at(i - 1).point.pose.position;
-    auto n = output->points.at(i + 1).point.pose.position;
-    double yaw = std::atan2(n.y - p.y, n.x - p.x);
-    output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
-  }
-  if (output->points.size() > 1) {
-    size_t l = output->points.size();
-    output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
-  }
-  return true;
-}
 
 NoStoppingAreaModule::NoStoppingAreaModule(
   const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
@@ -341,7 +267,8 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
   const double interpolation_interval = 0.5;
   bool is_in_area = false;
   autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path;
-  if (!splineInterpolate(path, interpolation_interval, &interpolated_path, logger_)) {
+  if (!interpolation::splineInterpolate(
+        path, interpolation_interval, &interpolated_path, logger_)) {
     return ego_area;
   }
   auto & pp = interpolated_path.points;

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -51,87 +51,6 @@ lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path)
   return lanelet::ConstLanelet(path_lanelet);
 }
 
-bool splineInterpolate(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
-{
-  *output = input;
-
-  if (input.points.size() <= 1) {
-    RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
-    return false;
-  }
-
-  static constexpr double ep = 1.0e-8;
-
-  // calc arclength for path
-  std::vector<double> base_x;
-  std::vector<double> base_y;
-  std::vector<double> base_z;
-  std::vector<double> base_v;
-  for (const auto & p : input.points) {
-    base_x.push_back(p.point.pose.position.x);
-    base_y.push_back(p.point.pose.position.y);
-    base_z.push_back(p.point.pose.position.z);
-    base_v.push_back(p.point.longitudinal_velocity_mps);
-  }
-  std::vector<double> base_s = interpolation::calcEuclidDist(base_x, base_y);
-
-  // remove duplicating sample points
-  {
-    size_t Ns = base_s.size();
-    size_t i = 1;
-    while (i < Ns) {
-      if (std::fabs(base_s[i - 1] - base_s[i]) < ep) {
-        base_s.erase(base_s.begin() + i);
-        base_x.erase(base_x.begin() + i);
-        base_y.erase(base_y.begin() + i);
-        base_z.erase(base_z.begin() + i);
-        base_v.erase(base_v.begin() + i);
-        Ns -= 1;
-        i -= 1;
-      }
-      ++i;
-    }
-  }
-
-  std::vector<double> resampled_s;
-  for (double d = 0.0; d < base_s.back() - ep; d += interval) {
-    resampled_s.push_back(d);
-  }
-
-  // do spline for xy
-  const std::vector<double> resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s);
-  const std::vector<double> resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s);
-  const std::vector<double> resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s);
-  const std::vector<double> resampled_v = ::interpolation::slerp(base_s, base_v, resampled_s);
-
-  // set xy
-  output->points.clear();
-  for (size_t i = 0; i < resampled_s.size(); i++) {
-    PathPointWithLaneId p;
-    p.point.pose.position.x = resampled_x.at(i);
-    p.point.pose.position.y = resampled_y.at(i);
-    p.point.pose.position.z = resampled_z.at(i);
-    p.point.longitudinal_velocity_mps = resampled_v.at(i);
-    output->points.push_back(p);
-  }
-
-  // set yaw
-  for (int i = 1; i < static_cast<int>(resampled_s.size()) - 1; i++) {
-    auto p = output->points.at(i - 1).point.pose.position;
-    auto n = output->points.at(i + 1).point.pose.position;
-    double yaw = std::atan2(n.y - p.y, n.x - p.x);
-    output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
-  }
-  if (output->points.size() > 1) {
-    size_t l = output->points.size();
-    output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
-  }
-  return true;
-}
-
 ROAD_TYPE getCurrentRoadType(
   const lanelet::ConstLanelet & current_lanelet,
   [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr)

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp

@@ -17,6 +17,7 @@
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 #include <scene_module/occlusion_spot/risk_predictive_braking.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp>
+#include <utilization/interpolate.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
@@ -73,7 +74,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
   utils::clipPathByLength(*path, clipped_path, max_range);
   PathWithLaneId interp_path;
   //! never change this interpolation interval(will affect module accuracy)
-  utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
+  interpolation::splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
   debug_data_.interp_path = interp_path;
   int closest_idx = -1;
   if (!planning_utils::calcClosestIndex<PathWithLaneId>(

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp

@@ -17,6 +17,7 @@
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 #include <scene_module/occlusion_spot/risk_predictive_braking.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp>
+#include <utilization/interpolate.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
@@ -72,7 +73,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   PathWithLaneId clipped_path;
   utils::clipPathByLength(*path, clipped_path, param_.detection_area_length);
   PathWithLaneId interp_path;
-  utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
+  interpolation::splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
   int closest_idx = -1;
   if (!planning_utils::calcClosestIndex<PathWithLaneId>(
         interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) {