feat(lane_change): separate execution and cancel safety check param (a…

…utowarefoundation#5246)

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -28,15 +28,23 @@
 
       # safety check
       safety_check:
-        expected_front_deceleration: -1.0
-        expected_rear_deceleration: -1.0
-        expected_front_deceleration_for_abort: -1.0
-        expected_rear_deceleration_for_abort: -2.0
-        rear_vehicle_reaction_time: 2.0
-        rear_vehicle_safety_time_margin: 1.0
-        lateral_distance_max_threshold: 2.0
-        longitudinal_distance_min_threshold: 3.0
-        longitudinal_velocity_delta_time: 0.8
+        allow_loose_check_for_cancel: true
+        execution:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -1.0
+          rear_vehicle_reaction_time: 2.0
+          rear_vehicle_safety_time_margin: 1.0
+          lateral_distance_max_threshold: 2.0
+          longitudinal_distance_min_threshold: 3.0
+          longitudinal_velocity_delta_time: 0.8
+        cancel:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -2.0
+          rear_vehicle_reaction_time: 1.5
+          rear_vehicle_safety_time_margin: 0.8
+          lateral_distance_max_threshold: 1.0
+          longitudinal_distance_min_threshold: 2.5
+          longitudinal_velocity_delta_time: 0.6
 
         # lane expansion for object filtering
         lane_expansion:

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -82,6 +82,7 @@ struct LaneChangeParameters
   utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 
   // safety check
+  bool allow_loose_check_for_cancel{true};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
 

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -92,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.stop_time_threshold =
     getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));
 
+  // safety check
+  p.allow_loose_check_for_cancel =
+    getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
+
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.longitudinal_distance_min_threshold"));
+    *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
+  p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
   p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.longitudinal_velocity_delta_time"));
-  p.rss_params.front_vehicle_deceleration =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.expected_front_deceleration"));
-  p.rss_params.rear_vehicle_deceleration =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.expected_rear_deceleration"));
-  p.rss_params.rear_vehicle_reaction_time =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
-  p.rss_params.rear_vehicle_safety_time_margin =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
-  p.rss_params.lateral_distance_max_threshold =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
+    *node, parameter("safety_check.execution.longitudinal_velocity_delta_time"));
+  p.rss_params.front_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.expected_front_deceleration"));
+  p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.expected_rear_deceleration"));
+  p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.rear_vehicle_reaction_time"));
+  p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin"));
+  p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.execution.lateral_distance_max_threshold"));
 
   p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.longitudinal_distance_min_threshold"));
+    *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold"));
   p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.longitudinal_velocity_delta_time"));
+    *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time"));
   p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.expected_front_deceleration_for_abort"));
+    *node, parameter("safety_check.cancel.expected_front_deceleration"));
   p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter<double>(
-    *node, parameter("safety_check.expected_rear_deceleration_for_abort"));
-  p.rss_params_for_abort.rear_vehicle_reaction_time =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
-  p.rss_params_for_abort.rear_vehicle_safety_time_margin =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
-  p.rss_params_for_abort.lateral_distance_max_threshold =
-    getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
+    *node, parameter("safety_check.cancel.expected_rear_deceleration"));
+  p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.cancel.rear_vehicle_reaction_time"));
+  p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.cancel.lateral_distance_max_threshold"));
 
   // target object
   {
@@ -165,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager(
     exit(EXIT_FAILURE);
   }
 
+  // validation of safety check parameters
+  // if loosely check is not allowed, lane change module will keep on chattering and canceling, and
+  // false positive situation might  occur
+  if (!p.allow_loose_check_for_cancel) {
+    if (
+      p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration ||
+      p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration ||
+      p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time ||
+      p.rss_params.rear_vehicle_safety_time_margin >
+        p.rss_params_for_abort.rear_vehicle_safety_time_margin ||
+      p.rss_params.lateral_distance_max_threshold >
+        p.rss_params_for_abort.lateral_distance_max_threshold ||
+      p.rss_params.longitudinal_distance_min_threshold >
+        p.rss_params_for_abort.longitudinal_distance_min_threshold ||
+      p.rss_params.longitudinal_velocity_delta_time >
+        p.rss_params_for_abort.longitudinal_velocity_delta_time) {
+      RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program...");
+      exit(EXIT_FAILURE);
+    }
+  }
   if (p.cancel.delta_time < 1.0) {
     RCLCPP_WARN_STREAM(
       logger_, "cancel.delta_time: " << p.cancel.delta_time