From eadb3e9f88a62d163c328b7626b75965ab002977 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 29 Jul 2022 12:19:59 +0900 Subject: [PATCH] Add longitudinal threshold and modify default param Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 3 ++- .../include/behavior_path_planner/parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 4 +++- planning/behavior_path_planner/src/utilities.cpp | 8 +++++--- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 82a65ab83a2f2..ba441ba9c6d3b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -20,7 +20,8 @@ visualize_drivable_area_for_shared_linestrings_lanelet: true lateral_distance_max_threshold: 5.0 + longitudinal_distance_min_threshold: 3.0 expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 2.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 6d9adf2b3cb23..423363fdf5b4b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -57,6 +57,7 @@ struct BehaviorPathPlannerParameters // collision check double lateral_distance_max_threshold; + double longitudinal_distance_min_threshold; double expected_front_deceleration; double expected_rear_deceleration; double rear_vehicle_reaction_time; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 316fc886dc68f..fe6b7d04fc688 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -211,9 +211,11 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0); + p.longitudinal_distance_min_threshold = + declare_parameter("longitudinal_distance_min_threshold", 3.0); p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0); p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0); - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 1.0); + p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0); p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); return p; } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index ac555751cb619..a6e8384e1399e 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2216,9 +2216,11 @@ bool hasEnoughDistance( util::l2Norm(front_vehicle_velocity), front_vehicle_accel, std::fabs(front_vehicle_pose.position.x)); - const auto rear_vehicle_stop_threshold = rearVehicleStopDistance( - util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time, - param.rear_vehicle_safety_time_margin); + const auto rear_vehicle_stop_threshold = std::max( + rearVehicleStopDistance( + util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time, + param.rear_vehicle_safety_time_margin), + param.longitudinal_distance_min_threshold); return isLongitudinalDistanceEnough(rear_vehicle_stop_threshold, front_vehicle_stop_threshold); }