add buffer

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Sep 5, 2023 · b4fa5b9 · b4fa5b9
1 parent e7c6108
commit b4fa5b9
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -1483,8 +1483,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 =

diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp
@@ -61,12 +61,16 @@ lanelet::ConstLanelets getPullOverLanes(
 {
   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.
+  // 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, backward_distance, forward_distance);
+      target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
   }
 
   lanelet::ConstLanelet closest_lane{};
@@ -80,7 +84,7 @@ lanelet::ConstLanelets getPullOverLanes(
 
   constexpr bool only_route_lanes = false;
   return route_handler.getLaneletSequence(
-    outermost_lane, backward_distance, forward_distance, only_route_lanes);
+    outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes);
 }
 
 PredictedObjects filterObjectsByLateralDistance(