diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index e50bc041cbc89..2c9a2c30b11b8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/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; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index daebfbed3d12a..b4e1f313a6936 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/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{ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 39a56977b301e..afba73ad45922 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/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 absence_traffic_light_creep_wall{std::nullopt}; + std::optional too_late_stop_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt};