Skip to content

Commit

Permalink
feat(lane_change): separate execution and cancel safety check param (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#5246)

Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Oct 6, 2023
1 parent 89fff0b commit ff04496
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 33 deletions.
26 changes: 17 additions & 9 deletions planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
5 changes: 3 additions & 2 deletions ..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,9 @@ struct LaneChangeParameters
utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;

// safety check
utils::path_safety_checker::RSSparams rss_params;
utils::path_safety_checker::RSSparams rss_params_for_abort;
bool allow_loose_check_for_cancel{true};
utils::path_safety_checker::RSSparams rss_params{};
utils::path_safety_checker::RSSparams rss_params_for_abort{};

// abort
LaneChangeCancelParameters cancel;
Expand Down
70 changes: 48 additions & 22 deletions planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,35 +89,41 @@ LaneChangeModuleManager::LaneChangeModuleManager(
p.stop_time_threshold =
getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));

// safety check
p.allow_loose_check_for_cancel =
getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));

p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*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<double>(
*node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_velocity_delta_time"));
p.rss_params.front_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter("safety_check.expected_front_deceleration"));
p.rss_params.rear_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter("safety_check.expected_rear_deceleration"));
p.rss_params.rear_vehicle_reaction_time =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
p.rss_params.rear_vehicle_safety_time_margin =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
p.rss_params.lateral_distance_max_threshold =
getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
*node, parameter("safety_check.execution.longitudinal_velocity_delta_time"));
p.rss_params.front_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.expected_front_deceleration"));
p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.expected_rear_deceleration"));
p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.rear_vehicle_reaction_time"));
p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.rear_vehicle_safety_time_margin"));
p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.lateral_distance_max_threshold"));

p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*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<double>(
*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<double>(
*node, parameter("safety_check.expected_front_deceleration_for_abort"));
*node, parameter("safety_check.cancel.expected_front_deceleration"));
p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.expected_rear_deceleration_for_abort"));
p.rss_params_for_abort.rear_vehicle_reaction_time =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
p.rss_params_for_abort.rear_vehicle_safety_time_margin =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
p.rss_params_for_abort.lateral_distance_max_threshold =
getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
*node, parameter("safety_check.cancel.expected_rear_deceleration"));
p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.rear_vehicle_reaction_time"));
p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin"));
p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.lateral_distance_max_threshold"));

// target object
{
Expand Down Expand Up @@ -162,6 +168,26 @@ 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
Expand Down

0 comments on commit ff04496

Please sign in to comment.