diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index a7ae4b496ef5d..7a4873c5dc66a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -48,8 +48,6 @@ class IntersectionModule : public SceneModuleInterface public: struct DebugData { - bool stop_required; - geometry_msgs::msg::Pose stop_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 861bcb05b726a..b2174e69512d8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -176,10 +176,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.stop_wall_pose}, "intersection", now, module_id_), - &wall_marker, now); + if (state_machine_.getState() == StateMachine::State::STOP) { + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.stop_wall_pose}, "intersection", now, module_id_), + &wall_marker, now); + } return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 5872972678685..359f3eeaa4765 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -271,7 +271,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * constexpr double v = 0.0; planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path); - debug_data_.stop_required = true; const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.stop_wall_pose = planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path);