add OccludedAbsenceTrafficLight state

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 3, 2023 · 29f9504 · 29f9504
1 parent 4d97ca3
commit 29f9504
Show file tree

Hide file tree

Showing 6 changed files with 173 additions and 68 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -54,6 +54,9 @@
         ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
         stop_release_margin_time: 1.5 # [s]
         temporal_stop_before_attention_area: false
+        absence_traffic_light:
+          creep_velocity: 1.388 # [m/s]
+          maximum_peeking_distance: 6.0 # [m]
 
       enable_rtc:
         intersection: true # 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_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp
@@ -271,29 +271,37 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
 
 motion_utils::VirtualWalls IntersectionModule::createVirtualWalls()
 {
-  // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance
   motion_utils::VirtualWalls virtual_walls;
   motion_utils::VirtualWall wall;
-  wall.style = motion_utils::VirtualWallType::stop;
 
   if (debug_data_.collision_stop_wall_pose) {
+    wall.style = motion_utils::VirtualWallType::stop;
     wall.text = "intersection";
     wall.ns = "intersection" + std::to_string(module_id_) + "_";
     wall.pose = debug_data_.collision_stop_wall_pose.value();
     virtual_walls.push_back(wall);
   }
   if (debug_data_.occlusion_first_stop_wall_pose) {
+    wall.style = motion_utils::VirtualWallType::stop;
     wall.text = "intersection";
     wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_";
     wall.pose = debug_data_.occlusion_first_stop_wall_pose.value();
     virtual_walls.push_back(wall);
   }
   if (debug_data_.occlusion_stop_wall_pose) {
+    wall.style = motion_utils::VirtualWallType::stop;
     wall.text = "intersection_occlusion";
     wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_";
     wall.pose = debug_data_.occlusion_stop_wall_pose.value();
     virtual_walls.push_back(wall);
   }
+  if (debug_data_.absence_traffic_light_creep_wall) {
+    wall.style = motion_utils::VirtualWallType::slowdown;
+    wall.text = "intersection_occlusion";
+    wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_";
+    wall.pose = debug_data_.absence_traffic_light_creep_wall.value();
+    virtual_walls.push_back(wall);
+  }
   return virtual_walls;
 }
 

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -136,8 +136,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".occlusion.stop_release_margin_time");
   ip.occlusion.temporal_stop_before_attention_area =
     getOrDeclareParameter<bool>(node, ns + ".occlusion.temporal_stop_before_attention_area");
-  ip.occlusion.peeking_offset_absence_tl =
-    getOrDeclareParameter<double>(node, ns + ".occlusion.peeking_offset_absence_tl");
+  ip.occlusion.absence_traffic_light.creep_velocity =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.absence_traffic_light.creep_velocity");
+  ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter<double>(
+    node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance");
 }
 
 void IntersectionModuleManager::launchNewModules(