From 922ec9070acf190a56fc01e67196b4efc367a50a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 6 Jun 2023 16:12:52 +0900 Subject: [PATCH] fix(lane_change): parameterize velocity scale for collision check (#3906) Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 1 + .../include/behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 ++ planning/behavior_path_planner/src/utils/safety_check.cpp | 3 +-- 4 files changed, 5 insertions(+), 2 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 d092feb2fc3de..a35f920dfb482 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -31,6 +31,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # [s] expected_front_deceleration: -1.0 expected_rear_deceleration: -1.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 ea6f709d02a15..aed5f04702028 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -145,6 +145,7 @@ struct BehaviorPathPlannerParameters // collision check double lateral_distance_max_threshold; double longitudinal_distance_min_threshold; + double longitudinal_velocity_delta_time; double expected_front_deceleration; // brake parameter under normal lane change double expected_rear_deceleration; // brake parameter under normal lane change 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 5ad9f2c2c4bf1..27e7a277ac766 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -475,6 +475,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); p.longitudinal_distance_min_threshold = declare_parameter("longitudinal_distance_min_threshold"); + p.longitudinal_velocity_delta_time = + declare_parameter("longitudinal_velocity_delta_time"); p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index cc5d698813df5..8fc4878c9d8f4 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -145,8 +145,7 @@ double calcMinimumLongitudinalLength( { const double & lon_threshold = params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - constexpr auto scale = 0.8; - return scale * std::abs(max_vel) + lon_threshold; + return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } bool isSafeInLaneletCollisionCheck(