From 6037589b634921483c90707a10d9e33c5fc78d5d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 21 Sep 2023 14:36:49 +0900 Subject: [PATCH] feat(start_planner): prevent start planner execution in the middle of the road (#5004) prevent start planner execution in the middle of the road Signed-off-by: Daniel Sanchez --- .../start_planner/start_planner.param.yaml | 1 + ...avior_path_planner_start_planner_design.md | 21 ++++++++++--------- .../start_planner_parameters.hpp | 1 + .../scene_module/start_planner/manager.cpp | 2 ++ .../start_planner/start_planner_module.cpp | 12 ++++++++--- 5 files changed, 24 insertions(+), 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 index 586676fbbb0f0..216ba4ccebbc9 100644 --- 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 index 2b98775e4fe81..0b0e01378b61a 100644 --- 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/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 771fbd93f7196..9e9173c3917a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_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 index 6d2f0aa50cb07..f5cd60e315e68 100644 --- 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(ns + "th_stopped_time"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( 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 index 619811430e535..89e9200fee250 100644 --- 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) {