fix(start_planner): fix geometric parallel parking lane ids (#4603)

* fix(start_planner): fix geometric parallel parking lane ids

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix stop path drivable lanes

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -143,7 +143,7 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose,
     const std::string search_priority);
   PathWithLaneId generateStopPath() const;
-  lanelet::ConstLanelets getPathLanes(const PathWithLaneId & path) const;
+  lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
   static bool isOverlappedWithLane(

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -534,19 +534,23 @@ PathWithLaneId StartPlannerModule::generateStopPath() const
   return path;
 }
 
-lanelet::ConstLanelets StartPlannerModule::getPathLanes(const PathWithLaneId & path) const
+lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId & path) const
 {
+  const auto & route_handler = planner_data_->route_handler;
+  const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer;
+
   std::vector<lanelet::Id> lane_ids;
   for (const auto & p : path.points) {
     for (const auto & id : p.lane_ids) {
+      if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) {
+        continue;
+      }
       if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) {
         lane_ids.push_back(id);
       }
     }
   }
 
-  const auto & lanelet_layer = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer;
-
   lanelet::ConstLanelets path_lanes;
   path_lanes.reserve(lane_ids.size());
   for (const auto & id : lane_ids) {
@@ -561,7 +565,22 @@ lanelet::ConstLanelets StartPlannerModule::getPathLanes(const PathWithLaneId & p
 std::vector<DrivableLanes> StartPlannerModule::generateDrivableLanes(
   const PathWithLaneId & path) const
 {
-  return utils::generateDrivableLanesWithShoulderLanes(getPathLanes(path), status_.pull_out_lanes);
+  const auto path_road_lanes = getPathRoadLanes(path);
+
+  if (!path_road_lanes.empty()) {
+    return utils::generateDrivableLanesWithShoulderLanes(
+      getPathRoadLanes(path), status_.pull_out_lanes);
+  }
+
+  // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes
+  std::vector<DrivableLanes> drivable_lanes;
+  for (const auto & lane : status_.pull_out_lanes) {
+    DrivableLanes drivable_lane;
+    drivable_lane.right_lane = lane;
+    drivable_lane.left_lane = lane;
+    drivable_lanes.push_back(drivable_lane);
+  }
+  return drivable_lanes;
 }
 
 void StartPlannerModule::updatePullOutStatus()
@@ -702,10 +721,15 @@ bool StartPlannerModule::hasFinishedPullOut() const
   const auto current_pose = planner_data_->self_odometry->pose.pose;
 
   // check that ego has passed pull out end point
-  const auto path_lanes = getPathLanes(getFullPath());
-  const auto arclength_current = lanelet::utils::getArcCoordinates(path_lanes, current_pose);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  const auto current_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*forward_only_in_route*/ true);
+
+  const auto arclength_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
   const auto arclength_pull_out_end =
-    lanelet::utils::getArcCoordinates(path_lanes, status_.pull_out_path.end_pose);
+    lanelet::utils::getArcCoordinates(current_lanes, status_.pull_out_path.end_pose);
 
   // offset to not finish the module before engage
   constexpr double offset = 0.1;

planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp

@@ -368,7 +368,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
 {
   clearPaths();
 
-  const auto common_params = planner_data_->parameters;
+  const auto & common_params = planner_data_->parameters;
+  const auto & route_handler = planner_data_->route_handler;
 
   const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0);
   const double self_yaw = tf2::getYaw(start_pose.orientation);
@@ -435,34 +436,49 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   PathWithLaneId path_turn_left = generateArcPath(
     Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward,
     is_forward);
-  path_turn_left.header = planner_data_->route_handler->getRouteHeader();
+  path_turn_left.header = route_handler->getRouteHeader();
 
   PathWithLaneId path_turn_right = generateArcPath(
     Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward,
     is_forward);
-  path_turn_right.header = planner_data_->route_handler->getRouteHeader();
+  path_turn_right.header = route_handler->getRouteHeader();
 
-  auto setLaneIds = [lanes](PathPointWithLaneId & p) {
-    for (const auto & lane : lanes) {
-      p.lane_ids.push_back(lane.id());
+  // Need to add straight path to last right_turning for parking in parallel
+  if (std::abs(end_pose_offset) > 0) {
+    PathPointWithLaneId straight_point{};
+    straight_point.point.pose = goal_pose;
+    // setLaneIds(straight_point);
+    path_turn_right.points.push_back(straight_point);
+  }
+
+  // Populate lane ids for a given path.
+  // It checks if each point in the path is within a lane
+  // and if its ID hasn't been added yet, it appends the ID to the container.
+  std::vector<lanelet::Id> path_lane_ids;
+  const auto populateLaneIds = [&](const auto & path) {
+    for (const auto & p : path.points) {
+      for (const auto & lane : lanes) {
+        if (
+          lanelet::utils::isInLanelet(p.point.pose, lane) &&
+          std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == path_lane_ids.end()) {
+          path_lane_ids.push_back(lane.id());
+        }
+      }
     }
   };
-  auto setLaneIdsToPath = [setLaneIds](PathWithLaneId & path) {
+  populateLaneIds(path_turn_left);
+  populateLaneIds(path_turn_right);
+
+  // Set lane ids to each point in a given path.
+  // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member.
+  const auto setLaneIdsToPath = [&](PathWithLaneId & path) {
     for (auto & p : path.points) {
-      setLaneIds(p);
+      p.lane_ids = path_lane_ids;
     }
   };
   setLaneIdsToPath(path_turn_left);
   setLaneIdsToPath(path_turn_right);
 
-  // Need to add straight path to last right_turning for parking in parallel
-  if (std::abs(end_pose_offset) > 0) {
-    PathPointWithLaneId straight_point{};
-    straight_point.point.pose = goal_pose;
-    setLaneIds(straight_point);
-    path_turn_right.points.push_back(straight_point);
-  }
-
   // generate arc path vector
   paths_.push_back(path_turn_left);
   paths_.push_back(path_turn_right);