Add longitudinal threshold and modify default param

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
autowarefoundation · Aug 17, 2022 · eadb3e9 · eadb3e9
1 parent a824a7b
commit eadb3e9
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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);
 }