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

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Jan 22, 2025 · 8ee41be · 8ee41be
1 parent 174a09e
commit 8ee41be
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 0 deletions.
diff --git a/...ng/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/...ng/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;
 }
 

diff --git a/...elocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/...elocity_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{

diff --git a/...elocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/...elocity_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};