From 955c3530db6a49ecadb91537d76e1c97bab96a5d Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Mon, 4 Mar 2024 11:42:54 +0900
Subject: [PATCH] feat(behavior_velocity_traffic_light): ensure stopping if a
 signal, once received, is not received again (autowarefoundation#6468)
 (#1156)

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

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



* add comments for clarity



* fix comments for clarity



---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 planning/behavior_velocity_planner/src/node.cpp     |  2 ++
 .../planner_data.hpp                                |  3 +++
 .../src/scene.cpp                                   | 13 +++++++++++--
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index eaeb84e70c500..7803942f2d16a 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -298,6 +298,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
+  planner_data_.has_received_signal_ = true;
+
   for (const auto & signal : msg->signals) {
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
index 08d3d0b512720..caeb134906608 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
@@ -89,6 +89,9 @@ struct PlannerData
   boost::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
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index 3dcb69273a18b..b42895c9927dd 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -302,11 +302,20 @@ bool TrafficLightModule::isStopSignal()
 {
   updateTrafficSignal();
 
-  // If it never receives traffic signal, it will PASS.
-  if (!traffic_signal_stamp_) {
+  // Pass through if no traffic signal information has been received yet
+  // This is to prevent stopping on the planning simulator
+  if (!planner_data_->has_received_signal_) {
     return false;
   }
 
+  // Stop if there is no upcoming traffic signal information
+  // This is to safely stop in cases such that traffic light recognition is not working properly or
+  // the map is incorrect
+  if (!traffic_signal_stamp_) {
+    return true;
+  }
+
+  // Stop if the traffic signal information has timed out
   if (isTrafficSignalTimedOut()) {
     return true;
   }