feat(start_planner): prevent start planner execution in the middle of… (

#860) feat(start_planner): prevent start planner execution in the middle of the road (autowarefoundation#5004) prevent start planner execution in the middle of the road Signed-off-by: Daniel Sanchez <[email protected]> Co-authored-by: danielsanchezaran <[email protected]>
tier4 · Sep 21, 2023 · 92e1f96 · 92e1f96
1 parent 75a263c
commit 92e1f96
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 13 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
@@ -7,6 +7,7 @@
       collision_check_margin: 1.0
       collision_check_distance_from_end: 1.0
       th_moving_object_velocity: 1.0
+      th_distance_to_middle_of_the_road: 0.1
       # shift pull out
       enable_shift_pull_out: true
       check_shift_path_lane_departure: false

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
@@ -59,16 +59,17 @@ PullOutPath --o PullOutPlannerBase
 
 ## General parameters for start_planner
 
-| Name                                                        | Unit  | Type   | Description                                                          | Default value |
-| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ |
-| th_arrived_distance_m                                       | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
-| th_stopped_velocity_mps                                     | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
-| th_stopped_time_sec                                         | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
-| th_turn_signal_on_lateral_offset                            | [m]   | double | lateral distance threshold for turning on blinker                    | 1.0           |
-| intersection_search_length                                  | [m]   | double | check if intersections exist within this length                      | 30.0          |
-| length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection              | 0.5           |
-| collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                      | 1.0           |
-| collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
+| Name                                                        | Unit  | Type   | Description                                                                 | Default value |
+| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ |
+| th_arrived_distance_m                                       | [m]   | double | distance threshold for arrival of path termination                          | 1.0           |
+| th_distance_to_middle_of_the_road                           | [m]   | double | distance threshold to determine if the vehicle is on the middle of the road |
+| th_stopped_velocity_mps                                     | [m/s] | double | velocity threshold for arrival of path termination                          | 0.01          |
+| th_stopped_time_sec                                         | [s]   | double | time threshold for arrival of path termination                              | 1.0           |
+| th_turn_signal_on_lateral_offset                            | [m]   | double | lateral distance threshold for turning on blinker                           | 1.0           |
+| intersection_search_length                                  | [m]   | double | check if intersections exist within this length                             | 30.0          |
+| length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection                     | 0.5           |
+| collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                             | 1.0           |
+| collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out        | 15.0          |
 
 ## Safety check with static obstacles
 

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
@@ -39,6 +39,7 @@ struct StartPlannerParameters
   double th_stopped_velocity;
   double th_stopped_time;
   double th_turn_signal_on_lateral_offset;
+  double th_distance_to_middle_of_the_road;
   double intersection_search_length;
   double length_ratio_for_turn_signal_deactivation_near_intersection;
   double collision_check_margin;

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
@@ -36,6 +36,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
   p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
   p.th_turn_signal_on_lateral_offset =
     node->declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
+  p.th_distance_to_middle_of_the_road =
+    node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
   p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
   p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -116,10 +116,16 @@ void StartPlannerModule::processOnExit()
 
 bool StartPlannerModule::isExecutionRequested() const
 {
-  // TODO(Sugahara): if required lateral shift distance is small, don't engage this module.
-  // Execute when current pose is near route start pose
-  const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
+  const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+  const double lateral_distance_to_center_lane =
+    lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
+
+  if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) {
+    return false;
+  }
+
+  const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   if (
     tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) >
     parameters_->th_arrived_distance) {