From d6d26ee21475d1d2a12389e20bb7d21356dbca9d Mon Sep 17 00:00:00 2001
From: Tomohito Ando <tomohito.ando@tier4.jp>
Date: Wed, 21 Feb 2024 15:22:26 +0900
Subject: [PATCH 1/3] 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>
---
 planning/behavior_velocity_planner/src/node.cpp            | 2 ++
 .../behavior_velocity_planner_common/planner_data.hpp      | 3 +++
 .../behavior_velocity_traffic_light_module/src/scene.cpp   | 7 ++++++-
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 9fa5634c6dd65..9f18a1a6de816 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/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 =
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 606e41ad4b1d1..23997e34fbfea 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
@@ -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
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index 96cb2c4e1c790..aaca066f88cbe 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/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;
   }

From 518cda64994a10e7230d798ce458f43bc2580112 Mon Sep 17 00:00:00 2001
From: Tomohito Ando <tomohito.ando@tier4.jp>
Date: Wed, 21 Feb 2024 15:23:50 +0900
Subject: [PATCH 2/3] add comments for clarity

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 planning/behavior_velocity_traffic_light_module/src/scene.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index aaca066f88cbe..3fdb5d514a0e9 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -291,6 +291,7 @@ bool TrafficLightModule::isStopSignal()
     return true;
   }
 
+  // If the signal data is timeout, the ego stops.
   if (isTrafficSignalTimedOut()) {
     return true;
   }

From ee820149faa55a274d8bb011c199ad1c90a18ee7 Mon Sep 17 00:00:00 2001
From: Tomohito Ando <tomohito.ando@tier4.jp>
Date: Thu, 22 Feb 2024 09:50:56 +0900
Subject: [PATCH 3/3] fix comments for clarity

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../src/scene.cpp                                      | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index 3fdb5d514a0e9..946741b14cf78 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -281,21 +281,25 @@ bool TrafficLightModule::isStopSignal()
 {
   updateTrafficSignal();
 
-  // If it never receives traffic signal, it will PASS.
+  // 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;
   }
 
-  // If the signal data is not received, the ego stops.
+  // 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;
   }
 
-  // If the signal data is timeout, the ego stops.
+  // Stop if the traffic signal information has timed out
   if (isTrafficSignalTimedOut()) {
     return true;
   }
 
+  // Check if the current traffic signal state requires stopping
   return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_);
 }