fix(behavior_path_planner): fix functions related to pull over lanes (#…

…4857)

* fix(behavior_path_planner): fix functions related to pull over lanes

Signed-off-by: kosuke55 <[email protected]>

* add buffer

Signed-off-by: kosuke55 <[email protected]>

* add comments

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp

@@ -46,7 +46,9 @@ using visualization_msgs::msg::MarkerArray;
 
 // TODO(sugahara) move to util
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
-lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side);
+lanelet::ConstLanelets getPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -335,8 +335,9 @@ bool GoalPlannerModule::isExecutionRequested() const
   // if (A) or (B) is met execute pull over
   // (A) target lane is `road` and same to the current lanes
   // (B) target lane is `road_shoulder` and neighboring to the current lanes
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
   if (!isCrossingPossible(current_lane, target_lane)) {
@@ -387,8 +388,9 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
   const double base_link2front = planner_data_->parameters.base_link2front;
   const double base_link2rear = planner_data_->parameters.base_link2rear;
 
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
 
   lanelet::Lanelet closest_pull_over_lanelet{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet);
@@ -643,8 +645,9 @@ void GoalPlannerModule::setLanes()
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
-  status_.pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  status_.pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   status_.lanes =
     utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes);
 }
@@ -1487,8 +1490,9 @@ bool GoalPlannerModule::isSafePath() const
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *route_handler, left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points);
   const auto & common_param = planner_data_->parameters;
   const std::pair<double, double> terminal_velocity_and_accel =
@@ -1655,8 +1659,9 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const
   const auto & route_handler = planner_data_->route_handler;
   const Pose & goal_pose = route_handler->getOriginalGoalPose();
 
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
 

planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp

@@ -63,8 +63,9 @@ boost::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
     /*forward_only_in_route*/ false);
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
+    parameters_.forward_goal_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }

planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp

@@ -47,8 +47,9 @@ boost::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
     /*forward_only_in_route*/ false);
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
+    parameters_.forward_goal_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }

planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

@@ -57,8 +57,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
   const double base_link2front = planner_data_->parameters.base_link2front;
   const double base_link2rear = planner_data_->parameters.base_link2rear;
 
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
+    parameters_.forward_goal_search_length);
   auto lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_length, forward_length,
     /*forward_only_in_route*/ false);
@@ -157,8 +158,9 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const
     }
 
     // check margin with pull over lane objects
-    const auto pull_over_lanes =
-      goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+    const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+      *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
+      parameters_.forward_goal_search_length);
     const auto [shoulder_lane_objects, others] =
       utils::path_safety_checker::separateObjectsByLanelets(
         *(planner_data_->dynamic_object), pull_over_lanes);

planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp

@@ -49,16 +49,16 @@ boost::optional<PullOverPath> ShiftPullOver::plan(const Pose & goal_pose)
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_search_length, forward_search_length,
     /*forward_only_in_route*/ false);
-  const auto shoulder_lanes =
-    goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
-  if (road_lanes.empty() || shoulder_lanes.empty()) {
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *route_handler, left_side_parking_, backward_search_length, forward_search_length);
+  if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
 
   // find safe one from paths with different jerk
   for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) {
     const auto pull_over_path =
-      generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk);
+      generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk);
     if (!pull_over_path) continue;
     return *pull_over_path;
   }

planning/behavior_path_planner/src/utils/goal_planner/util.cpp

@@ -55,36 +55,37 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith
   return path;
 }
 
-lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side)
+lanelet::ConstLanelets getPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance)
 {
   const Pose goal_pose = route_handler.getOriginalGoalPose();
 
+  // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance.
+  // In the case of loop lanes, it may not be possible to extend the lane forward.
+  // todo(kosuek55): automatically calculates this distance.
+  const double backward_distance_with_buffer = backward_distance + 100;
+
   lanelet::ConstLanelet target_shoulder_lane{};
   if (route_handler::RouteHandler::getPullOverTarget(
         route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) {
     // pull over on shoulder lane
-    return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose);
+    return route_handler.getShoulderLaneletSequence(
+      target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
   }
 
-  // pull over on road lane
   lanelet::ConstLanelet closest_lane{};
   route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane);
-
   lanelet::ConstLanelet outermost_lane{};
   if (left_side) {
-    outermost_lane = route_handler.getMostLeftLanelet(closest_lane);
+    outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true);
   } else {
-    outermost_lane = route_handler.getMostRightLanelet(closest_lane);
-  }
-
-  lanelet::ConstLanelet outermost_shoulder_lane;
-  if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) {
-    return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose);
+    outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true);
   }
 
-  const bool dist = std::numeric_limits<double>::max();
   constexpr bool only_route_lanes = false;
-  return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes);
+  return route_handler.getLaneletSequence(
+    outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes);
 }
 
 PredictedObjects filterObjectsByLateralDistance(

planning/route_handler/include/route_handler/route_handler.hpp

@@ -122,7 +122,8 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   boost::optional<lanelet::ConstLanelet> getRightLanelet(
-    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false,
+    const bool get_shoulder_lane = false) const;
 
   /**
    * @brief Check if same-direction lane is available at the left side of the lanelet
@@ -132,7 +133,8 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   boost::optional<lanelet::ConstLanelet> getLeftLanelet(
-    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false,
+    const bool get_shoulder_lane = false) const;
   lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const;
 
@@ -195,7 +197,8 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   lanelet::ConstLanelet getMostRightLanelet(
-    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false,
+    const bool get_shoulder_lane = false) const;
 
   /**
    * @brief Check if same-direction lane is available at the left side of the lanelet
@@ -205,7 +208,8 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   lanelet::ConstLanelet getMostLeftLanelet(
-    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false,
+    const bool get_shoulder_lane = false) const;
 
   /**
    * @brief Searches the furthest linestring to the right side of the lanelet

planning/route_handler/src/route_handler.cpp

@@ -528,7 +528,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter(
   while (rclcpp::ok() && length < min_length) {
     lanelet::ConstLanelet next_lanelet;
     if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) {
-      break;
+      if (only_route_lanes) {
+        break;
+      }
+      const auto next_lanes = getNextLanelets(current_lanelet);
+      if (next_lanes.empty()) {
+        break;
+      }
+      next_lanelet = next_lanes.front();
     }
     // loop check
     if (lanelet.id() == next_lanelet.id()) {
@@ -556,7 +563,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
   while (rclcpp::ok() && length < min_length) {
     lanelet::ConstLanelets candidate_lanelets;
     if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) {
-      break;
+      if (only_route_lanes) {
+        break;
+      }
+      const auto prev_lanes = getPreviousLanelets(current_lanelet);
+      if (prev_lanes.empty()) {
+        break;
+      }
+      candidate_lanelets = prev_lanes;
     }
     // loop check
     if (std::any_of(
@@ -954,7 +968,8 @@ bool RouteHandler::isBijectiveConnection(
 }
 
 boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
-  const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
+  const lanelet::ConstLanelet & lanelet, const bool enable_same_root,
+  const bool get_shoulder_lane) const
 {
   // right road lanelet of shoulder lanelet
   if (isShoulderLanelet(lanelet)) {
@@ -966,6 +981,14 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
     return boost::none;
   }
 
+  // right shoulder lanelet
+  if (get_shoulder_lane) {
+    lanelet::ConstLanelet right_shoulder_lanelet;
+    if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) {
+      return right_shoulder_lanelet;
+    }
+  }
+
   // routable lane
   const auto & right_lane = routing_graph_ptr_->right(lanelet);
   if (right_lane) {
@@ -1026,7 +1049,8 @@ bool RouteHandler::getLeftLaneletWithinRoute(
 }
 
 boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
-  const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
+  const lanelet::ConstLanelet & lanelet, const bool enable_same_root,
+  const bool get_shoulder_lane) const
 {
   // left road lanelet of shoulder lanelet
   if (isShoulderLanelet(lanelet)) {
@@ -1038,6 +1062,14 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
     return boost::none;
   }
 
+  // left shoulder lanelet
+  if (get_shoulder_lane) {
+    lanelet::ConstLanelet left_shoulder_lanelet;
+    if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) {
+      return left_shoulder_lanelet;
+    }
+  }
+
   // routable lane
   const auto & left_lane = routing_graph_ptr_->left(lanelet);
   if (left_lane) {
@@ -1213,26 +1245,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane
 }
 
 lanelet::ConstLanelet RouteHandler::getMostRightLanelet(
-  const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
+  const lanelet::ConstLanelet & lanelet, const bool enable_same_root,
+  const bool get_shoulder_lane) const
 {
   // recursively compute the width of the lanes
-  const auto & same = getRightLanelet(lanelet, enable_same_root);
+  const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane);
 
   if (same) {
-    return getMostRightLanelet(same.get(), enable_same_root);
+    return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane);
   }
 
   return lanelet;
 }
 
 lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(
-  const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
+  const lanelet::ConstLanelet & lanelet, const bool enable_same_root,
+  const bool get_shoulder_lane) const
 {
   // recursively compute the width of the lanes
-  const auto & same = getLeftLanelet(lanelet, enable_same_root);
+  const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane);
 
   if (same) {
-    return getMostLeftLanelet(same.get(), enable_same_root);
+    return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane);
   }
 
   return lanelet;