Add expanded drivable lanes to ShiftPullOut constructor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp

@@ -34,7 +34,7 @@ class ShiftPullOut : public PullOutPlannerBase
 public:
   explicit ShiftPullOut(
     rclcpp::Node & node, const StartPlannerParameters & parameters,
-    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
+    std::shared_ptr<LaneDepartureChecker> & lane_departure_checker, const lanelet::ConstLanelets & expanded_drivable_lanes);
 
   PlannerType getPlannerType() override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
@@ -51,6 +51,7 @@ class ShiftPullOut : public PullOutPlannerBase
     const double longitudinal_acc, const double lateral_acc);
 
   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

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -242,6 +242,7 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
+  lanelet::ConstLanelets createExpandedDrivableLanes();
 
   // check if the goal is located behind the ego in the same route segment.
   bool isGoalBehindOfEgoInSameRouteSegment() const;

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -37,8 +37,8 @@ using start_planner_utils::getPullOutLanes;
 
 ShiftPullOut::ShiftPullOut(
   rclcpp::Node & node, const StartPlannerParameters & parameters,
-  std::shared_ptr<LaneDepartureChecker> & lane_departure_checker)
-: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}
+  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}
 {
 }
 
@@ -49,14 +49,11 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
 
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
-  const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length);
-  if (pull_out_lanes.empty()) {
-    return std::nullopt;
-  }
-
   const auto road_lanes = utils::getExtendedCurrentLanes(
     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);
   if (pull_out_paths.empty()) {
@@ -69,40 +66,18 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
       pull_out_path.partial_paths.front();  // shift path is not separate but only one.
 
     // check lane_departure with path between pull_out_start to pull_out_end
-    PathWithLaneId path_start_to_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);
 
-      // calculate collision check end idx
-      const size_t collision_check_end_idx = std::invoke([&]() {
-        const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
-          shift_path.points, pull_out_path.end_pose.position,
-          parameters_.collision_check_distance_from_end);
-
-        if (collision_check_end_pose) {
-          return findNearestIndex(shift_path.points, collision_check_end_pose->position);
-        } else {
-          return shift_path.points.size() - 1;
-        }
-      });
-      path_start_to_end.points.insert(
-        path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
-        shift_path.points.begin() + collision_check_end_idx + 1);
+      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);
     }
 
-    // extract shoulder lanes from pull out lanes
-    lanelet::ConstLanelets shoulder_lanes;
-    std::copy_if(
-      pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
-      [&route_handler](const auto & pull_out_lane) {
-        return 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));
+
 
     // crop backward path
     // removes points which are out of lanes up to the start pose.
@@ -117,7 +92,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_lanes, transformed_vehicle_footprint);
+        LaneDepartureChecker::isOutOfLane(expanded_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));
@@ -131,7 +106,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_lanes, path_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(expanded_drivable_lanes_, path_shift_start_to_end)) {
       continue;
     }
 

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_));
+    start_planners_.push_back(std::make_shared<ShiftPullOut>(
+      node, *parameters, lane_departure_checker_, createExpandedDrivableLanes()));
   }
   if (parameters_->enable_geometric_pull_out) {
     start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters));
@@ -1325,6 +1325,34 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
+lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes()
+{
+  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;
+  // }
+  const auto road_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*forward_only_in_route*/ true);
+  // extract shoulder lanes from pull out lanes
+  lanelet::ConstLanelets shoulder_lanes;
+  std::copy_if(
+    pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
+    [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;
+}
+
 void StartPlannerModule::setDebugData()
 {
   using marker_utils::addFootprintMarker;