feat(intersection): add wall marker for too late detect objects (autowarefoundation#10006)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp

@@ -446,6 +446,14 @@ autoware::motion_utils::VirtualWalls IntersectionModule::createVirtualWalls()
     wall.pose = debug_data_.absence_traffic_light_creep_wall.value();
     virtual_walls.push_back(wall);
   }
+  if (debug_data_.too_late_stop_wall_pose) {
+    wall.style = autoware::motion_utils::VirtualWallType::pass;
+    wall.text = "intersection";
+    wall.detail = "too late to stop";
+    wall.ns = "intersection" + std::to_string(module_id_) + "_";
+    wall.pose = debug_data_.too_late_stop_wall_pose.value();
+    virtual_walls.push_back(wall);
+  }
   return virtual_walls;
 }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -273,6 +273,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat
       const auto closest_idx = intersection_stoplines.closest_idx;
       const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis(
         *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects);
+      debug_data_.too_late_stop_wall_pose = path->points.at(default_stopline_idx).point.pose;
       return OverPassJudge{safety_diag, evasive_diag};
     }
     return OverPassJudge{

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -214,6 +214,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC
     bool passed_first_pass_judge{false};
     bool passed_second_pass_judge{false};
     std::optional<geometry_msgs::msg::Pose> absence_traffic_light_creep_wall{std::nullopt};
+    std::optional<geometry_msgs::msg::Pose> too_late_stop_wall_pose{std::nullopt};
 
     std::optional<std::vector<lanelet::CompoundPolygon3d>> attention_area{std::nullopt};
     std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};