Refactor ShiftPullOut class and update expanded drivable lanes

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
autowarefoundation · Jan 18, 2024 · 6cd1904 · 6cd1904
1 parent 18dc35e
commit 6cd1904
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 15 deletions.
diff --git a/...r_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/...r_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp
@@ -34,8 +34,7 @@ class ShiftPullOut : public PullOutPlannerBase
 public:
   explicit ShiftPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
-    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
-    const lanelet::ConstLanelets & expanded_drivable_lanes);
+    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
@@ -51,15 +50,21 @@ class ShiftPullOut : public PullOutPlannerBase
     ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose,
     const double longitudinal_acc, const double lateral_acc);
 
+  void setExpandedDrivableLanes(const lanelet::ConstLanelets & expanded_drivable_lanes)
+  {
+    expanded_drivable_lanes_ = expanded_drivable_lanes;
+  }
+
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
-  lanelet::ConstLanelets expanded_drivable_lanes_;
 
 private:
   // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the
   // minimum distance requirement.
   double calcPullOutLongitudinalDistance(
     const double lon_acc, const double shift_time, const double shift_length,
     const double max_curvature, const double min_distance) const;
+
+  lanelet::ConstLanelets expanded_drivable_lanes_;
 };
 }  // namespace behavior_path_planner
 

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -242,7 +242,8 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
-  lanelet::ConstLanelets createExpandedDrivableLanes();
+  void updateExpandedDrivableLanes();
+  lanelet::ConstLanelets createExpandedDrivableLanes() const;
 
   // check if the goal is located behind the ego in the same route segment.
   bool isGoalBehindOfEgoInSameRouteSegment() const;

diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
@@ -37,11 +37,8 @@ using start_planner_utils::getPullOutLanes;
 
 ShiftPullOut::ShiftPullOut(
   rclcpp::Node & node, const StartPlannerParameters & parameters,
-  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker,
-  const lanelet::ConstLanelets & expanded_drivable_lanes)
-: PullOutPlannerBase{node, parameters},
-  lane_departure_checker_{lane_departure_checker},
-  expanded_drivable_lanes_{expanded_drivable_lanes}
+  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker)
+: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}
 {
 }
 

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -61,8 +61,8 @@ StartPlannerModule::StartPlannerModule(
 
   // set enabled planner
   if (parameters_->enable_shift_pull_out) {
-    start_planners_.push_back(std::make_shared<ShiftPullOut>(
-      node, *parameters, lane_departure_checker_, createExpandedDrivableLanes()));
+    start_planners_.push_back(
+      std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (parameters_->enable_geometric_pull_out) {
     start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters));
@@ -127,6 +127,7 @@ void StartPlannerModule::initVariables()
   debug_marker_.markers.clear();
   initializeSafetyCheckParameters();
   initializeCollisionCheckDebugMap(start_planner_data_.collision_check);
+  updateExpandedDrivableLanes();
 }
 
 void StartPlannerModule::updateEgoPredictedPathParams(
@@ -559,6 +560,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 void StartPlannerModule::resetStatus()
 {
   status_ = PullOutStatus{};
+  updateExpandedDrivableLanes();
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -1325,15 +1327,27 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
-lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes()
+void StartPlannerModule::updateExpandedDrivableLanes()
+{
+  const auto expanded_drivable_lanes = createExpandedDrivableLanes();
+  for (auto & planner : start_planners_) {
+    auto shift_pull_out = std::dynamic_pointer_cast<ShiftPullOut>(planner);
+
+    if (shift_pull_out) {
+      shift_pull_out->setExpandedDrivableLanes(expanded_drivable_lanes);
+    }
+  }
+}
+
+lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const
 {
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
-  // if (pull_out_lanes.empty()) {
-  //   return std::nullopt;
-  // }
+  if (pull_out_lanes.empty()) {
+    return lanelet::ConstLanelets{};
+  }
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);