feat(start_planner): add option for lane departure

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jul 3, 2023 · 45f0ef0 · 45f0ef0
1 parent 47db336
commit 45f0ef0
Show file tree

Hide file tree

Showing 5 changed files with 9 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
@@ -8,6 +8,7 @@
       collision_check_distance_from_end: 1.0
       # shift pull out
       enable_shift_pull_out: true
+      check_shift_path_lane_departure: false
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0
       lateral_jerk: 0.5

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
@@ -105,6 +105,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
 | Name                            | Unit   | Type   | Description                                                                                                          | Default value |
 | :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
 | enable_shift_pull_out           | [-]    | bool   | flag whether to enable shift pull out                                                                                | true          |
+| check_shift_path_lane_departure | [-]    | bool   | flag whether to check if shift path footprints are out of lane                                                       | false         |
 | shift_pull_out_velocity         | [m/s]  | double | velocity of shift pull out                                                                                           | 2.0           |
 | pull_out_sampling_num           | [-]    | int    | Number of samplings in the minimum to maximum range of lateral_jerk                                                  | 4             |
 | maximum_lateral_jerk            | [m/s3] | double | maximum lateral jerk                                                                                                 | 2.0           |

diff --git a/...th_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/...th_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp
@@ -35,6 +35,7 @@ struct StartPlannerParameters
   double collision_check_distance_from_end;
   // shift pull out
   bool enable_shift_pull_out;
+  bool check_shift_path_lane_departure;
   double minimum_shift_pull_out_distance;
   int lateral_acceleration_sampling_num;
   double lateral_jerk;

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp
@@ -44,6 +44,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   // shift pull out
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
+  p.check_shift_path_lane_departure =
+    node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
   p.minimum_shift_pull_out_distance =
     node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
   p.lateral_acceleration_sampling_num =

diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -100,8 +100,10 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
     const auto expanded_lanes = utils::expandLanelets(
       drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
       dp.drivable_area_types_to_skip);
-    if (lane_departure_checker_->checkPathWillLeaveLane(
-          utils::transformToLanelets(expanded_lanes), path_start_to_end)) {
+    if (
+      parameters_.check_shift_path_lane_departure &&
+      lane_departure_checker_->checkPathWillLeaveLane(
+        utils::transformToLanelets(expanded_lanes), path_start_to_end)) {
       continue;
     }