Refactor drivable lane functions

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

Hide file tree

Showing 4 changed files with 14 additions and 19 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
@@ -52,7 +52,7 @@ class ShiftPullOut : public PullOutPlannerBase
 
   void setExpandedDrivableLanes(const lanelet::ConstLanelets & expanded_drivable_lanes)
   {
-    expanded_drivable_lanes_ = expanded_drivable_lanes;
+    drivable_lanes_ = expanded_drivable_lanes;
   }
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
@@ -64,7 +64,7 @@ class ShiftPullOut : public PullOutPlannerBase
     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_;
+  lanelet::ConstLanelets 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,8 +242,8 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
-  void updateExpandedDrivableLanes();
-  lanelet::ConstLanelets createExpandedDrivableLanes() const;
+  void updateDrivableLanes();
+  lanelet::ConstLanelets createDrivableLanes() 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
@@ -98,7 +98,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
       const auto transformed_vehicle_footprint =
         transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose));
       const bool is_out_of_lane =
-        LaneDepartureChecker::isOutOfLane(expanded_drivable_lanes_, transformed_vehicle_footprint);
+        LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint);
       if (i <= start_segment_idx) {
         if (!is_out_of_lane) {
           cropped_path.points.push_back(shift_path.points.at(i));
@@ -112,8 +112,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     // check lane departure
     if (
       parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(
-        expanded_drivable_lanes_, path_shift_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_start_to_end)) {
       continue;
     }
 

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
@@ -127,7 +127,7 @@ void StartPlannerModule::initVariables()
   debug_marker_.markers.clear();
   initializeSafetyCheckParameters();
   initializeCollisionCheckDebugMap(start_planner_data_.collision_check);
-  updateExpandedDrivableLanes();
+  updateDrivableLanes();
 }
 
 void StartPlannerModule::updateEgoPredictedPathParams(
@@ -560,7 +560,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 void StartPlannerModule::resetStatus()
 {
   status_ = PullOutStatus{};
-  updateExpandedDrivableLanes();
+  updateDrivableLanes();
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -1327,9 +1327,9 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
-void StartPlannerModule::updateExpandedDrivableLanes()
+void StartPlannerModule::updateDrivableLanes()
 {
-  const auto expanded_drivable_lanes = createExpandedDrivableLanes();
+  const auto expanded_drivable_lanes = createDrivableLanes();
   for (auto & planner : start_planners_) {
     auto shift_pull_out = std::dynamic_pointer_cast<ShiftPullOut>(planner);
 
@@ -1339,7 +1339,7 @@ void StartPlannerModule::updateExpandedDrivableLanes()
   }
 }
 
-lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const
+lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const
 {
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
@@ -1358,13 +1358,9 @@ lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const
     [this](const auto & pull_out_lane) {
       return planner_data_->route_handler->isShoulderLanelet(pull_out_lane);
     });
-  const auto drivable_lanes =
-    utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
-  const auto & dp = planner_data_->drivable_area_expansion_parameters;
-  const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets(
-    drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-    dp.drivable_area_types_to_skip));
-  return expanded_lanes;
+  const auto drivable_lanes = utils::transformToLanelets(
+    utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes));
+  return drivable_lanes;
 }
 
 void StartPlannerModule::setDebugData()