From b5e22ea40d7619c27395f802ddb9b9e99b1cff4b Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Aug 2024 17:17:41 +0900 Subject: [PATCH] different rss value for parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi --- .../utils/data_structs.hpp | 1 + .../src/manager.cpp | 17 +++++++++++++++++ .../src/scene.cpp | 7 ++++++- 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 495fe9012ecd0..f0971b93a6057 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -151,6 +151,7 @@ struct Parameters bool allow_loose_check_for_cancel{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_parked{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 22c93f7d4bfd5..184b682d88a39 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -122,6 +122,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); + p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_front_deceleration")); + p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_rear_deceleration")); + p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); + p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); + p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.lateral_distance_max_threshold")); + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 97eab5a03fcaa..5f63e5ca60685 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -2069,9 +2069,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; + const auto selected_rss_param = + (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, current_debug_data.second);