refactor(autoware_behavior_path_start_planner_module): move planner_data to method parameter

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp

@@ -43,7 +43,9 @@ class FreespacePullOut : public PullOutPlannerBase
   PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
 
   std::optional<PullOutPath> plan(
-    const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override;
+    const Pose & start_pose, const Pose & end_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
+    PlannerDebugData & planner_debug_data) override;
 
 protected:
   std::unique_ptr<AbstractPlanningAlgorithm> planner_;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp

@@ -40,6 +40,7 @@ class GeometricPullOut : public PullOutPlannerBase
   PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(
     const Pose & start_pose, const Pose & goal_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
     PlannerDebugData & planner_debug_data) override;
 
   GeometricParallelParking planner_;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp

@@ -41,33 +41,30 @@ class PullOutPlannerBase
   explicit PullOutPlannerBase(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
-  : time_keeper_(time_keeper)
+  : vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()},
+    vehicle_footprint_{vehicle_info_.createFootprint()},
+    parameters_{parameters},
+    time_keeper_(time_keeper)
   {
-    vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
-    vehicle_footprint_ = vehicle_info_.createFootprint();
-    parameters_ = parameters;
   }
   virtual ~PullOutPlannerBase() = default;
 
-  void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
-  {
-    planner_data_ = planner_data;
-  }
-
   void setCollisionCheckMargin(const double collision_check_margin)
   {
     collision_check_margin_ = collision_check_margin;
   };
   virtual PlannerType getPlannerType() const = 0;
   virtual std::optional<PullOutPath> plan(
-    const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0;
+    const Pose & start_pose, const Pose & goal_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
+    PlannerDebugData & planner_debug_data) = 0;
 
 protected:
   bool isPullOutPathCollided(
     autoware::behavior_path_planner::PullOutPath & pull_out_path,
+    const std::shared_ptr<const PlannerData> & planner_data,
     double collision_check_distance_from_end) const;
 
-  std::shared_ptr<const PlannerData> planner_data_;
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   LinearRing2d vehicle_footprint_;
   StartPlannerParameters parameters_;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp

@@ -41,11 +41,13 @@ class ShiftPullOut : public PullOutPlannerBase
   PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(
     const Pose & start_pose, const Pose & goal_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
     PlannerDebugData & planner_debug_data) override;
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-    const Pose & start_pose, const Pose & goal_pose);
+    const Pose & start_pose, const Pose & goal_pose,
+    const BehaviorPathPlannerParameters & behavior_path_parameters);
 
   double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp

@@ -51,13 +51,14 @@ FreespacePullOut::FreespacePullOut(
 }
 
 std::optional<PullOutPath> FreespacePullOut::plan(
-  const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data)
+  const Pose & start_pose, const Pose & end_pose,
+  const std::shared_ptr<const PlannerData> & planner_data, PlannerDebugData & planner_debug_data)
 {
-  const auto & route_handler = planner_data_->route_handler;
-  const double backward_path_length = planner_data_->parameters.backward_path_length;
-  const double forward_path_length = planner_data_->parameters.forward_path_length;
+  const auto & route_handler = planner_data->route_handler;
+  const double backward_path_length = planner_data->parameters.backward_path_length;
+  const double forward_path_length = planner_data->parameters.forward_path_length;
 
-  planner_->setMap(*planner_data_->costmap);
+  planner_->setMap(*planner_data->costmap);
 
   try {
     if (!planner_->makePlan(start_pose, end_pose)) {
@@ -69,11 +70,11 @@ std::optional<PullOutPath> FreespacePullOut::plan(
   }
 
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    planner_data, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
   // find candidate paths
   const auto pull_out_lanes =
-    start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
+    start_planner_utils::getPullOutLanes(planner_data, backward_path_length);
   const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes);
 
   const PathWithLaneId path =

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -47,24 +47,25 @@ GeometricPullOut::GeometricPullOut(
 }
 
 std::optional<PullOutPath> GeometricPullOut::plan(
-  const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data)
+  const Pose & start_pose, const Pose & goal_pose,
+  const std::shared_ptr<const PlannerData> & planner_data, PlannerDebugData & planner_debug_data)
 {
   PullOutPath output;
 
   // combine road lane and pull out lane
   const double backward_path_length =
-    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+    planner_data->parameters.backward_path_length + parameters_.max_back_distance;
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    planner_data, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
-  const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length);
+  const auto pull_out_lanes = getPullOutLanes(planner_data, backward_path_length);
 
   // check if the ego is at left or right side of road lane center
   const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance;
 
   planner_.setTurningRadius(
-    planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
-  planner_.setPlannerData(planner_data_);
+    planner_data->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
+  planner_.setPlannerData(planner_data);
   const bool found_valid_path = planner_.planPullOut(
     start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_);
   if (!found_valid_path) {
@@ -122,7 +123,8 @@ std::optional<PullOutPath> GeometricPullOut::plan(
   output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose;
   output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose;
 
-  if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) {
+  if (isPullOutPathCollided(
+        output, planner_data, parameters_.geometric_collision_check_distance_from_end)) {
     planner_debug_data.conditions_evaluation.emplace_back("collision");
     return {};
   }

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp

@@ -14,21 +14,24 @@
 
 #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp"
 
+#include <memory>
+
 namespace autoware::behavior_path_planner
 {
 bool PullOutPlannerBase::isPullOutPathCollided(
   autoware::behavior_path_planner::PullOutPath & pull_out_path,
+  const std::shared_ptr<const PlannerData> & planner_data,
   double collision_check_distance_from_end) const
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   // check for collisions
-  const auto & dynamic_objects = planner_data_->dynamic_object;
+  const auto & dynamic_objects = planner_data->dynamic_object;
   if (!dynamic_objects) {
     return false;
   }
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
-    planner_data_, planner_data_->parameters.backward_path_length + parameters_.max_back_distance);
+    planner_data, planner_data->parameters.backward_path_length + parameters_.max_back_distance);
   // extract stop objects in pull out lane for collision check
   const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *dynamic_objects, parameters_.th_moving_object_velocity);

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -48,120 +48,123 @@
 }
 
 std::optional<PullOutPath> ShiftPullOut::plan(
-  const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data)
+  const Pose & start_pose, const Pose & goal_pose,
+  const std::shared_ptr<const PlannerData> & planner_data, PlannerDebugData & planner_debug_data)
 {
-  const auto & route_handler = planner_data_->route_handler;
-  const auto & common_parameters = planner_data_->parameters;
+  const auto & route_handler = planner_data->route_handler;
+  const auto & common_parameters = planner_data->parameters;
 
   const double backward_path_length =
-    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+    planner_data->parameters.backward_path_length + parameters_.max_back_distance;
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    planner_data, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
   // find candidate paths
-  auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose);
+  auto pull_out_paths =
+    calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose, common_parameters);
   if (pull_out_paths.empty()) {
     planner_debug_data.conditions_evaluation.emplace_back("no path found");
     return std::nullopt;
   }
 
-  const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+  const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr();
 
   std::vector<lanelet::Id> fused_id_start_to_end{};
   std::optional<autoware::universe_utils::Polygon2d> fused_polygon_start_to_end = std::nullopt;
 
   std::vector<lanelet::Id> fused_id_crop_points{};
   std::optional<autoware::universe_utils::Polygon2d> fused_polygon_crop_points = std::nullopt;
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
     universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_);
 
     // shift path is not separate but only one.
     auto & shift_path = pull_out_path.partial_paths.front();
     // check lane_departure with path between pull_out_start to pull_out_end
     PathWithLaneId path_shift_start_to_end{};
     {
       const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position);
       const size_t pull_out_end_idx =
         findNearestIndex(shift_path.points, pull_out_path.end_pose.position);
 
       path_shift_start_to_end.points.insert(
         path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
         shift_path.points.begin() + pull_out_end_idx + 1);
     }
 
     // if lane departure check override is true, and if the initial pose is not fully within a lane,
     // cancel lane departure check
     const bool is_lane_departure_check_required = std::invoke([&]() -> bool {
       if (!parameters_.allow_check_shift_path_lane_departure_override)
         return parameters_.check_shift_path_lane_departure;
 
       PathWithLaneId path_with_only_first_pose{};
       path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.front());
       return !lane_departure_checker_->checkPathWillLeaveLane(
         lanelet_map_ptr, path_with_only_first_pose);
     });
 
     // check lane departure
     // The method for lane departure checking verifies if the footprint of each point on the path
     // is contained within a lanelet using `boost::geometry::within`, which incurs a high
     // computational cost.
 
     if (
       is_lane_departure_check_required && lane_departure_checker_->checkPathWillLeaveLane(
                                             lanelet_map_ptr, path_shift_start_to_end,
                                             fused_id_start_to_end, fused_polygon_start_to_end)) {
       planner_debug_data.conditions_evaluation.emplace_back("lane departure");
       continue;
     }
 
     // crop backward path
     // removes points which are out of lanes up to the start pose.
     // this ensures that the backward_path stays within the drivable area when starting from a
     // narrow place.
 
     const size_t start_segment_idx =
       autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
         shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold,
         common_parameters.ego_nearest_yaw_threshold);
 
     const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes(
       lanelet_map_ptr, shift_path, start_segment_idx, fused_id_crop_points,
       fused_polygon_crop_points);
     if (cropped_path.points.empty()) {
       planner_debug_data.conditions_evaluation.emplace_back("cropped path is empty");
       continue;
     }
 
     // check that the path is not cropped in excess and there is not excessive longitudinal
     // deviation between the first 2 points
     auto validate_cropped_path = [&](const auto & cropped_path) -> bool {
       if (cropped_path.points.size() < 2) return false;
       const double max_long_offset = parameters_.maximum_longitudinal_deviation;
       const size_t start_segment_idx_after_crop =
         autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
           cropped_path.points, start_pose);
 
       // if the start segment id after crop is not 0, then the cropping is not excessive
       if (start_segment_idx_after_crop != 0) return true;
 
       const auto long_offset_to_closest_point =
         autoware::motion_utils::calcLongitudinalOffsetToSegment(
           cropped_path.points, start_segment_idx_after_crop, start_pose.position);
       const auto long_offset_to_next_point =
         autoware::motion_utils::calcLongitudinalOffsetToSegment(
           cropped_path.points, start_segment_idx_after_crop + 1, start_pose.position);
       return std::abs(long_offset_to_closest_point - long_offset_to_next_point) < max_long_offset;
     };
 
     if (!validate_cropped_path(cropped_path)) {
       planner_debug_data.conditions_evaluation.emplace_back("cropped path is invalid");
       continue;
     }
     shift_path.points = cropped_path.points;
-    shift_path.header = planner_data_->route_handler->getRouteHeader();
+    shift_path.header = planner_data->route_handler->getRouteHeader();
 
-    if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) {
+    if (isPullOutPathCollided(
+          pull_out_path, planner_data, parameters_.shift_collision_check_distance_from_end)) {
       planner_debug_data.conditions_evaluation.emplace_back("collision");
       continue;
     }
@@ -227,20 +230,20 @@
 
 std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-  const Pose & start_pose, const Pose & goal_pose)
+  const Pose & start_pose, const Pose & goal_pose,
+  const BehaviorPathPlannerParameters & behavior_path_parameters)
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   std::vector<PullOutPath> candidate_paths{};
 
   if (road_lanes.empty()) {
     return candidate_paths;
   }
 
   // rename parameter
-  const auto & common_parameters = planner_data_->parameters;
-  const double forward_path_length = common_parameters.forward_path_length;
-  const double backward_path_length = common_parameters.backward_path_length;
+  const double forward_path_length = behavior_path_parameters.forward_path_length;
+  const double backward_path_length = behavior_path_parameters.backward_path_length;
   const double lateral_jerk = parameters_.lateral_jerk;
   const double minimum_lateral_acc = parameters_.minimum_lateral_acc;
   const double maximum_lateral_acc = parameters_.maximum_lateral_acc;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -986,17 +986,16 @@ bool StartPlannerModule::findPullOutPath(
   const bool backward_is_unnecessary = backwards_distance < epsilon;
 
   planner->setCollisionCheckMargin(collision_check_margin);
-  planner->setPlannerData(planner_data_);
   PlannerDebugData debug_data{
     planner->getPlannerType(), {}, collision_check_margin, backwards_distance};
 
-  const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, debug_data);
+  const auto pull_out_path =
+    planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data);
   debug_data_vector.push_back(debug_data);
   // If no path is found, return false
   if (!pull_out_path) {
     return false;
   }
-
   if (backward_is_unnecessary) {
     updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType());
     return true;
@@ -1595,9 +1594,9 @@ std::optional<PullOutStatus> StartPlannerModule::planFreespacePath(
 
   for (const auto & p : center_line_path.points) {
     const Pose end_pose = p.point.pose;
-    freespace_planner_->setPlannerData(planner_data);
     PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0};
-    auto freespace_path = freespace_planner_->plan(current_pose, end_pose, debug_data);
+    auto freespace_path =
+      freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data);
     DEBUG_PRINT(
       "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(),
       debug_data.str().c_str());