feat(behavior_velocity_planner): use sampling functions from motion u…

…tils (tier4#1520)

planning/behavior_velocity_planner/include/utilization/path_utilization.hpp

@@ -27,7 +27,7 @@ namespace behavior_velocity_planner
 {
 bool splineInterpolate(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger);
+  autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger);
 autoware_auto_planning_msgs::msg::Path interpolatePath(
   const autoware_auto_planning_msgs::msg::Path & path, const double length,
   const double interval = 1.0);

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

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

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

@@ -189,7 +189,7 @@ bool generateStopLine(
 
   /* spline interpolation */
   autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!splineInterpolate(target_path, interval, &path_ip, logger)) {
+  if (!splineInterpolate(target_path, interval, path_ip, logger)) {
     return false;
   }
 
@@ -561,7 +561,7 @@ bool generateStopLineBeforeIntersection(
 
   /* spline interpolation */
   autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!splineInterpolate(input_path, interval, &path_ip, logger)) {
+  if (!splineInterpolate(input_path, interval, path_ip, logger)) {
     return false;
   }
   const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);

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

@@ -279,7 +279,7 @@ 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 (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) {
     return ego_area;
   }
   auto & pp = interpolated_path.points;

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

@@ -104,7 +104,7 @@ bool OcclusionSpotModule::modifyPathVelocity(
   utils::clipPathByLength(*path, clipped_path, param_.detection_area_length);
   PathWithLaneId path_interpolated;
   //! never change this interpolation interval(will affect module accuracy)
-  splineInterpolate(clipped_path, 1.0, &path_interpolated, logger_);
+  splineInterpolate(clipped_path, 1.0, path_interpolated, logger_);
   const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position;
   const auto offset = motion_utils::calcSignedArcLength(
     path_interpolated.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr);

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -37,91 +37,15 @@ namespace behavior_velocity_planner
 {
 bool splineInterpolate(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
+  autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger)
 {
-  *output = input;
-
-  if (input.points.size() <= 1) {
+  if (input.points.size() < 2) {
     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 = calcEuclidDist(base_x, base_y);
+  output = motion_utils::resamplePath(input, interval, false, true, true, false);
 
-  // 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::lerp(base_s, base_z, resampled_s);
-  const std::vector<double> resampled_v =
-    ::interpolation::zero_order_hold(base_s, base_v, 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);
-    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 =
-      tier4_autoware_utils::createQuaternionFromYaw(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;
-  }
-
-  // insert final point on path if its position is not same point as interpolated
-  const auto & op = output->points.back().point.pose.position;
-  const auto & ip = input.points.back().point.pose.position;
-  if (std::hypot(op.x - ip.x, op.y - ip.y) > ep) {
-    output->points.emplace_back(input.points.back());
-  }
   return true;
 }