chore(intersection): display intersection marker only when stopped (#…

…3150) Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>
autowarefoundation · Mar 24, 2023 · e0e2a60 · e0e2a60
1 parent 075b8f4
commit e0e2a60
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 7 deletions.
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
@@ -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
@@ -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
@@ -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);