refactor(behavior_velocity_planner): move interpolated path to PlannerData

planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml

@@ -12,6 +12,7 @@
     launch_run_out: false
     forward_path_length: 1000.0
     backward_path_length: 5.0
+    interpolate_interval: 0.2
     max_accel: -2.8
     max_jerk: -5.0
     system_delay: 0.5

planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

@@ -50,6 +50,7 @@
 #include <deque>
 #include <map>
 #include <memory>
+#include <optional>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -65,6 +66,7 @@ struct PlannerData
     max_stop_jerk_threshold = node.declare_parameter("max_jerk", -5.0);
     system_delay = node.declare_parameter("system_delay", 0.50);
     delay_response_time = node.declare_parameter("delay_response_time", 0.50);
+    interpolate_interval = node.declare_parameter("interpolate_interval", 0.2);
   }
   // tf
   geometry_msgs::msg::PoseStamped current_pose;
@@ -79,6 +81,9 @@ struct PlannerData
   // occupancy grid
   nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
 
+  // interpolation
+  std::optional<autoware_auto_planning_msgs::msg::PathWithLaneId> interpolated_path;
+
   // nearest search
   double ego_nearest_dist_threshold;
   double ego_nearest_yaw_threshold;
@@ -106,6 +111,7 @@ struct PlannerData
   double system_delay;
   double delay_response_time;
   double stop_line_extend_length;
+  double interpolate_interval;
 
   bool isVehicleStopped(const double stop_duration = 0.0) const
   {

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

@@ -76,9 +76,8 @@ bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   const double keep_detection_line_margin,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path,
-  StopLineIdx * stop_line_idxs, const rclcpp::Logger logger);
+  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, StopLineIdx * stop_line_idxs,
+  const rclcpp::Logger logger);
 
 /**
  * @brief If use_stuck_stopline is true, a stop line is generated before the intersection.
@@ -91,7 +90,6 @@ bool generateStopLine(
 bool generateStopLineBeforeIntersection(
   const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const std::shared_ptr<const PlannerData> & planner_data,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
   autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
   int * pass_judge_line_idx, const rclcpp::Logger logger);
 

planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp

@@ -136,7 +136,6 @@ class NoStoppingAreaModule : public SceneModuleInterface
    * @return generated polygon
    */
   Polygon2d generateEgoNoStoppingAreaLanePolygon(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const;
 
   /**

planning/behavior_velocity_planner/include/utilization/path_utilization.hpp

@@ -21,13 +21,14 @@
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <optional>
 #include <vector>
 
 namespace behavior_velocity_planner
 {
-bool splineInterpolate(
+std::optional<autoware_auto_planning_msgs::msg::PathWithLaneId> splineInterpolate(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger);
+  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/node.cpp

@@ -32,6 +32,7 @@
 
 #include <functional>
 #include <memory>
+#include <optional>
 
 // Scene modules
 #include <scene_module/blind_spot/manager.hpp>
@@ -414,6 +415,10 @@ void BehaviorVelocityPlannerNode::onTrigger(
     return;
   }
 
+  // spline interpolate
+  planner_data_.interpolated_path =
+    splineInterpolate(*input_path_msg, planner_data_.interpolate_interval, get_logger());
+
   if (!isDataReady(planner_data_, *get_clock())) {
     mutex_.unlock();
     return;

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

@@ -227,17 +227,16 @@ bool BlindSpotModule::generateStopLine(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const
 {
   /* set parameters */
-  constexpr double interval = 0.2;
+  const double interval = planner_data_->interpolate_interval;
   const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval);
   const int base2front_idx_dist =
     std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval);
 
   /* spline interpolation */
-  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!splineInterpolate(*path, interval, path_ip, logger_)) {
+  if (!planner_data_->interpolated_path.has_value()) {
     return false;
   }
-  debug_data_.spline_path = path_ip;
+  const auto & path_ip = planner_data_->interpolated_path.value();
 
   /* generate stop point */
   int stop_idx_ip = 0;  // stop point index for interpolated path.

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -291,11 +291,12 @@ boost::optional<geometry_msgs::msg::Point> CrosswalkModule::findNearestStopPoint
   bool found_pedestrians = false;
   bool found_stuck_vehicle = false;
 
-  PathWithLaneId sparse_resample_path{};
   constexpr double RESAMPLE_INTERVAL = 4.0;
-  if (!splineInterpolate(ego_path, RESAMPLE_INTERVAL, sparse_resample_path, logger_)) {
+  const auto sparse_resample_path_opt = splineInterpolate(ego_path, RESAMPLE_INTERVAL, logger_);
+  if (!sparse_resample_path_opt.has_value()) {
     return {};
   }
+  const PathWithLaneId & sparse_resample_path = sparse_resample_path_opt.value();
 
   const auto crosswalk_attention_range = getAttentionRange(sparse_resample_path);
   const auto & ego_pos = planner_data_->current_pose.pose.position;

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

@@ -128,7 +128,7 @@ bool IntersectionModule::modifyPathVelocity(
                                   : detection_area;
   if (!util::generateStopLine(
         lane_id_, attention_area, planner_data_, planner_param_.stop_line_margin,
-        planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
+        planner_param_.keep_detection_line_margin, path, &stop_line_idxs,
         logger_.get_child("util"))) {
     // returns here if path is not intersecting with attention_area
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
@@ -226,7 +226,7 @@ bool IntersectionModule::modifyPathVelocity(
       int stuck_stop_line_idx = -1;
       int stuck_pass_judge_line_idx = -1;
       if (util::generateStopLineBeforeIntersection(
-            lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx,
+            lane_id_, lanelet_map_ptr, planner_data_, path, &stuck_stop_line_idx,
             &stuck_pass_judge_line_idx, logger_.get_child("util"))) {
         stop_line_idx_stop = stuck_stop_line_idx;
         pass_judge_line_idx_stop = stuck_pass_judge_line_idx;

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

@@ -79,11 +79,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
 
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
-  const auto private_path =
-    extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
   if (!util::generateStopLine(
         lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
-        0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
+        0.0 /* unnecessary in merge_from_private */, path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     return false;

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

@@ -119,9 +119,8 @@ bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   const double keep_detection_line_margin,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path,
-  StopLineIdx * stop_line_idxs, const rclcpp::Logger logger)
+  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, StopLineIdx * stop_line_idxs,
+  const rclcpp::Logger logger)
 {
   /* set judge line dist */
   const double current_vel = planner_data->current_velocity->twist.linear.x;
@@ -133,12 +132,12 @@ bool generateStopLine(
     current_vel, current_acc, max_acc, max_jerk, delay_response_time);
 
   /* spline interpolation */
-  constexpr double interval = 0.2;
-  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
+  const double interval = planner_data->interpolate_interval;
   // TODO(Mamoru Sobue): crop only intersection part of path for computation cost
-  if (!splineInterpolate(target_path, interval, path_ip, logger)) {
+  if (!planner_data->interpolated_path.has_value()) {
     return false;
   }
+  const auto & path_ip = planner_data->interpolated_path.value();
 
   const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval);
   const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_line_margin / interval);
@@ -428,7 +427,6 @@ std::vector<int> getLaneletIdsFromLanelets(lanelet::ConstLanelets ll)
 bool generateStopLineBeforeIntersection(
   const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const std::shared_ptr<const PlannerData> & planner_data,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
   autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
   int * pass_judge_line_idx, const rclcpp::Logger logger)
 {
@@ -448,10 +446,10 @@ bool generateStopLineBeforeIntersection(
   const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval);
 
   /* spline interpolation */
-  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
-  if (!splineInterpolate(input_path, interval, path_ip, logger)) {
+  if (!planner_data->interpolated_path.has_value()) {
     return false;
   }
+  const auto & path_ip = planner_data->interpolated_path.value();
   const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);
   for (size_t i = 0; i < path_ip.points.size(); i++) {
     const auto & p = path_ip.points.at(i).point.pose;

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

@@ -151,13 +151,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(
   const double ego_space_in_front_of_stuck_vehicle =
     margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin;
   const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon(
-    *path, current_pose.pose, ego_space_in_front_of_stuck_vehicle,
-    planner_param_.detection_area_length);
+    current_pose.pose, ego_space_in_front_of_stuck_vehicle, planner_param_.detection_area_length);
   const double ego_space_in_front_of_stop_line =
     margin + planner_param_.stop_margin + vi.rear_overhang_m;
   const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon(
-    *path, current_pose.pose, ego_space_in_front_of_stop_line,
-    planner_param_.detection_area_length);
+    current_pose.pose, ego_space_in_front_of_stop_line, planner_param_.detection_area_length);
   if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) {
     setSafe(true);
     return true;
@@ -275,17 +273,15 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
 }
 
 Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const
 {
   Polygon2d ego_area;  // open polygon
   double dist_from_start_sum = 0.0;
-  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 (!planner_data_->interpolated_path.has_value()) {
     return ego_area;
   }
+  const auto & interpolated_path = planner_data_->interpolated_path.value();
   auto & pp = interpolated_path.points;
   /* calc closest index */
   const auto closest_idx_opt =

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

@@ -102,9 +102,8 @@ bool OcclusionSpotModule::modifyPathVelocity(
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
   PathWithLaneId clipped_path;
   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_);
+  const PathWithLaneId & path_interpolated = splineInterpolate(clipped_path, 1.0, logger_).value();
   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/scene_module/run_out/dynamic_obstacle.cpp

@@ -182,11 +182,12 @@ pcl::PointCloud<pcl::PointXYZ> extractLateralNearestPoints(
   const float interval)
 {
   // interpolate path points with given interval
-  PathWithLaneId interpolated_path;
-  if (!splineInterpolate(
-        path, interval, interpolated_path, rclcpp::get_logger("dynamic_obstacle_creator"))) {
+  const auto interpolated_path_opt =
+    splineInterpolate(path, interval, rclcpp::get_logger("dynamic_obstacle_creator"));
+  if (!interpolated_path_opt.has_value()) {
     return input_points;
   }
+  const auto & interpolated_path = interpolated_path_opt.value();
 
   // divide points into groups according to nearest segment index
   const auto points_with_index =
@@ -360,6 +361,8 @@ void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud(
     extractObstaclePointsWithinPolygon(voxel_grid_filtered_points, detection_area_polygon);
 
   // filter points that have lateral nearest distance
+  // TODO(Mamour Sobue): check the smallest interval of this module and use interpolated path in
+  // planner_data
   const auto lateral_nearest_points =
     extractLateralNearestPoints(detection_area_filtered_points, path, param_.points_interval);
 

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -35,18 +35,16 @@ constexpr double DOUBLE_EPSILON = 1e-6;
 
 namespace behavior_velocity_planner
 {
-bool splineInterpolate(
+std::optional<autoware_auto_planning_msgs::msg::PathWithLaneId> splineInterpolate(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger)
+  const rclcpp::Logger logger)
 {
   if (input.points.size() < 2) {
     RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
-    return false;
+    return std::nullopt;
   }
 
-  output = motion_utils::resamplePath(input, interval, false, true, true, false);
-
-  return true;
+  return motion_utils::resamplePath(input, interval, false, true, true, false);
 }
 
 /*