fix(lane_change): parameterize velocity scale for collision check (au…

…towarefoundation#3906) Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
brkay54 · Jun 6, 2023 · 922ec90 · 922ec90
1 parent 62850d0
commit 922ec90
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 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
@@ -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
@@ -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
@@ -475,6 +475,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.lateral_distance_max_threshold = declare_parameter<double>("lateral_distance_max_threshold");
   p.longitudinal_distance_min_threshold =
     declare_parameter<double>("longitudinal_distance_min_threshold");
+  p.longitudinal_velocity_delta_time =
+    declare_parameter<double>("longitudinal_velocity_delta_time");
 
   p.expected_front_deceleration = declare_parameter<double>("expected_front_deceleration");
   p.expected_rear_deceleration = declare_parameter<double>("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
@@ -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(