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 f97c30d448cf7..68592e338c2d9 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 @@ -93,7 +93,8 @@ LaneChangeModuleManager::LaneChangeModuleManager( 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.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.execution.longitudinal_distance_min_threshold")); @@ -101,16 +102,16 @@ LaneChangeModuleManager::LaneChangeModuleManager( *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.execution.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.execution.expected_rear_deceleration")); - p.rss_params.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.execution.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.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.execution.rear_vehicle_safety_time_margin")); + p.rss_params.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.cancel.longitudinal_distance_min_threshold")); @@ -120,12 +121,12 @@ LaneChangeModuleManager::LaneChangeModuleManager( *node, parameter("safety_check.cancel.expected_front_deceleration_for_abort")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( *node, parameter("safety_check.cancel.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.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.cancel.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + p.rss_params_for_abort.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.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -171,19 +172,23 @@ LaneChangeModuleManager::LaneChangeModuleManager( } // 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 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) {