feat(behavior_velocity_traffic_light): ensure stopping if a signal, once received, is not received again."

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

Author: TomohitoAndo

planning/behavior_velocity_planner/src/node.cpp

@@ -323,6 +323,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
+  planner_data_.has_received_signal_ = true;
+
   // clear previous observation
   planner_data_.traffic_light_id_map_raw_.clear();
   const auto traffic_light_id_map_last_observed_old =

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -83,6 +83,9 @@ struct PlannerData
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
+  // this value becomes true once the signal message is received
+  bool has_received_signal_ = false;
+
   // velocity smoother
   std::shared_ptr<motion_velocity_smoother::SmootherBase> velocity_smoother_;
   // route handler

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -282,10 +282,15 @@ bool TrafficLightModule::isStopSignal()
   updateTrafficSignal();
 
   // If it never receives traffic signal, it will PASS.
-  if (!traffic_signal_stamp_) {
+  if (!planner_data_->has_received_signal_) {
     return false;
   }
 
+  // If the signal data is not received, the ego stops.
+  if (!traffic_signal_stamp_) {
+    return true;
+  }
+
   if (isTrafficSignalTimedOut()) {
     return true;
   }