From 513916e7aec138f37241e294a53cabefed5cdbfa Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 6 Oct 2023 15:28:52 +0900 Subject: [PATCH] feat(lane_change): separate execution and cancel safety check param Signed-off-by: Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 26 ++++++---- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 49 +++++++++++++------ 3 files changed, 53 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index fda7390abdb9e..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,23 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0ba35635a9143..24a507d1f8695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -82,6 +82,7 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check + bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 9c45f79d00263..f97c30d448cf7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -92,35 +92,40 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); + getOrDeclareParameter(*node, parameter("safety_check.execution.expected_front_deceleration")); p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); + getOrDeclareParameter(*node, parameter("safety_check.execution.expected_rear_deceleration")); p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); + getOrDeclareParameter(*node, parameter("safety_check.execution.rear_vehicle_reaction_time")); p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); + getOrDeclareParameter(*node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + getOrDeclareParameter(*node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration_for_abort")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_rear_deceleration_for_abort")); p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); + getOrDeclareParameter(*node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); + getOrDeclareParameter(*node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + getOrDeclareParameter(*node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -165,6 +170,22 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and false positive situation might occur + if(!p.allow_loose_check_for_cancel){ + if(p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > p.rss_params_for_abort.longitudinal_velocity_delta_time){ + + RCLCPP_FATAL_STREAM( + logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time