feat(behavior_velocity_blind_spot): use TTC for object in detection_a…

…rea (autowarefoundation#7146) Signed-off-by: Mamoru Sobue <[email protected]>
tier4 · May 29, 2024 · 08f108d · 08f108d
1 parent ba48569
commit 08f108d
Show file tree

Hide file tree

Showing 5 changed files with 170 additions and 214 deletions.
diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml
@@ -3,10 +3,12 @@
     blind_spot:
       use_pass_judge_line: true
       stop_line_margin: 1.0 # [m]
-      backward_length: 50.0 # [m]
+      backward_detection_length: 100.0 # [m]
       ignore_width_from_center_line: 0.7 # [m]
-      max_future_movement_time: 10.0 # [second]
-      threshold_yaw_diff: 0.523 # [rad]
       adjacent_extend_width: 1.5 # [m]
       opposite_adjacent_extend_width: 1.5 # [m]
+      max_future_movement_time: 10.0 # [second]
+      ttc_min: -5.0 # [s]
+      ttc_max: 5.0 # [s]
+      ttc_ego_minimal_velocity: 5.0 # [m/s]
       enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp
@@ -93,15 +93,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
 
   const auto now = this->clock_->now();
 
-  appendMarkerArray(
-    createLaneletPolygonsMarkerArray(
-      debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),
-    &debug_marker_array, now);
-
-  appendMarkerArray(
-    createLaneletPolygonsMarkerArray(
-      debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0),
-    &debug_marker_array, now);
+  if (debug_data_.detection_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0),
+      &debug_marker_array, now);
+  }
 
   appendMarkerArray(
     debug::createObjectsMarkerArray(

diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp
@@ -36,17 +36,20 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
   planner_param_.use_pass_judge_line =
     getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
   planner_param_.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
-  planner_param_.backward_length = getOrDeclareParameter<double>(node, ns + ".backward_length");
+  planner_param_.backward_detection_length =
+    getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
   planner_param_.ignore_width_from_center_line =
     getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
-  planner_param_.max_future_movement_time =
-    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
-  planner_param_.threshold_yaw_diff =
-    getOrDeclareParameter<double>(node, ns + ".threshold_yaw_diff");
   planner_param_.adjacent_extend_width =
     getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
   planner_param_.opposite_adjacent_extend_width =
     getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
+  planner_param_.max_future_movement_time =
+    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
+  planner_param_.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
+  planner_param_.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
+  planner_param_.ttc_ego_minimal_velocity =
+    getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
 }
 
 void BlindSpotModuleManager::launchNewModules(