feat(start_planner): add option for lane departure (autowarefoundatio…

…n#4151) Signed-off-by: kosuke55 <[email protected]> shift
tier4 · Sep 12, 2023 · dd508d0 · dd508d0
1 parent 4d5708b
commit dd508d0
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
@@ -9,6 +9,7 @@
       th_moving_object_velocity: 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
@@ -46,6 +46,7 @@ struct StartPlannerParameters
   double th_moving_object_velocity;
   // 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/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -1067,6 +1067,8 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
   p.th_moving_object_velocity = declare_parameter<double>(ns + "th_moving_object_velocity");
   // shift pull out
   p.enable_shift_pull_out = declare_parameter<bool>(ns + "enable_shift_pull_out");
+  p.check_shift_path_lane_departure =
+    declare_parameter<bool>(ns + "check_shift_path_lane_departure");
   p.minimum_shift_pull_out_distance =
     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
@@ -103,8 +103,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;
     }