feat(goal_planner): expand pull over lanes for detection area of path…

… generation collision check Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jan 19, 2024 · 11fcfb1 · 11fcfb1
1 parent c833bea
commit 11fcfb1
Show file tree

Hide file tree

Showing 5 changed files with 12 additions and 1 deletion.
diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md
@@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 | use_object_recognition                                       | [-]  | bool   | flag whether to use object recognition for collision check                                               | true          |
 | object_recognition_collision_check_margin                    | [m]  | double | margin to calculate ego-vehicle cells from footprint.                                                    | 0.6           |
 | object_recognition_collision_check_max_extra_stopping_margin | [m]  | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0           |
+| detection_bound_offset                                       | [m]  | double | expand pull over lane with this offset to make detection area for collision check of path generation     | 15.0          |
 
 ## **Goal Search**
 

diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -38,6 +38,7 @@
         object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker
         object_recognition_collision_check_max_extra_stopping_margin: 1.0
         th_moving_object_velocity: 1.0
+        detection_bound_offset: 15.0
 
       # pull over
       pull_over:

diff --git a/...goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/...goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp
@@ -74,6 +74,7 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_margin{0.0};
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};
   double th_moving_object_velocity{0.0};
+  double detection_bound_offset{0.0};
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};

diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1487,9 +1487,16 @@ bool GoalPlannerModule::checkObjectsCollision(
   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 expanded_pull_over_lanes =
+    left_side_parking_ ? lanelet::utils::getExpandedLanelets(
+                           pull_over_lanes, parameters_->detection_bound_offset, 0.0)
+                       : lanelet::utils::getExpandedLanelets(
+                           pull_over_lanes, 0.0, parameters_->detection_bound_offset);
+
   const auto [pull_over_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *(planner_data_->dynamic_object), pull_over_lanes,
+      *(planner_data_->dynamic_object), expanded_pull_over_lanes,
       utils::path_safety_checker::isPolygonOverlapLanelet);
   const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_over_lane_objects, parameters_->th_moving_object_velocity);

diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp
@@ -101,6 +101,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(
         ns + "object_recognition_collision_check_max_extra_stopping_margin");
     p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
+    p.detection_bound_offset = node->declare_parameter<double>(ns + "detection_bound_offset");
   }
 
   // pull over general params