From f713b7bfb9a847a463085650b2b99787a72faba5 Mon Sep 17 00:00:00 2001
From: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Date: Wed, 22 Jan 2025 18:52:38 +0900
Subject: [PATCH 01/10] chore(sync-files.yaml): not synchronize
 `github-release.yaml` (#1776)

not sync github-release

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
---
 .github/sync-files.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
index 4c09e072766fa..8afb7671215f4 100644
--- a/.github/sync-files.yaml
+++ b/.github/sync-files.yaml
@@ -15,7 +15,6 @@
     - source: .github/workflows/comment-on-pr.yaml
     - source: .github/workflows/delete-closed-pr-docs.yaml
     - source: .github/workflows/deploy-docs.yaml
-    - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml
     - source: .github/workflows/pre-commit-optional.yaml
     - source: .github/workflows/pre-commit-optional-autoupdate.yaml

From 0609609db1348d8234502ba9e272d37cfb4382b2 Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Wed, 29 Jan 2025 17:59:23 +0900
Subject: [PATCH 02/10] feat(autoware_behavior_velocity_traffic_light_module):
 add v2i to behavior_velocity_traffic_light

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../config/traffic_light.param.yaml           |  6 +++
 .../package.xml                               |  1 +
 .../src/manager.cpp                           | 53 +++++++++++++++++++
 .../src/manager.hpp                           | 12 +++++
 .../src/scene.cpp                             | 33 +++++++++++-
 .../src/scene.hpp                             | 10 ++++
 6 files changed, 114 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml
index 23746a61b6043..6aac965227efb 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml
@@ -7,3 +7,9 @@
       yellow_lamp_period: 2.75
       enable_pass_judge: true
       enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
+
+      v2i:
+        use_rest_time: false
+        last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red
+        velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not
+        required_time_to_departure: 3.0 # prevent low speed pass
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml
index 518d3275a28d3..16f6e85c422d6 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml
@@ -31,6 +31,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>jpn_signal_v2i_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
index 1d1ee7fc996b2..a8279972d55f3 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
@@ -17,14 +17,17 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
+#include <rclcpp/logging.hpp>
 
 #include <tf2/utils.h>
 
 #include <limits>
 #include <memory>
+#include <optional>
 #include <set>
 #include <string>
 #include <utility>
+#include <vector>
 namespace autoware::behavior_velocity_planner
 {
 using autoware::universe_utils::getOrDeclareParameter;
@@ -42,8 +45,22 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   planner_param_.enable_pass_judge = getOrDeclareParameter<bool>(node, ns + ".enable_pass_judge");
   planner_param_.yellow_lamp_period =
     getOrDeclareParameter<double>(node, ns + ".yellow_lamp_period");
+  planner_param_.v2i_use_rest_time = getOrDeclareParameter<bool>(node, ns + ".v2i.use_rest_time");
+  planner_param_.v2i_last_time_allowed_to_pass =
+    getOrDeclareParameter<double>(node, ns + ".v2i.last_time_allowed_to_pass");
+  planner_param_.v2i_velocity_threshold =
+    getOrDeclareParameter<double>(node, ns + ".v2i.velocity_threshold");
+  planner_param_.v2i_required_time_to_departure =
+    getOrDeclareParameter<double>(node, ns + ".v2i.required_time_to_departure");
   pub_tl_state_ = node.create_publisher<autoware_perception_msgs::msg::TrafficLightGroup>(
     "~/output/traffic_signal", 1);
+
+  if (planner_param_.v2i_use_rest_time) {
+    RCLCPP_INFO(logger_, "V2I is enabled");
+    v2i_subscriber_ = autoware::universe_utils::InterProcessPollingSubscriber<
+      jpn_signal_v2i_msgs::msg::TrafficLightInfo>::
+      create_subscription(&node, "/v2i/external/v2i_traffic_light_info", 1);
+  }
 }
 
 void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path)
@@ -53,6 +70,8 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
 
   autoware_perception_msgs::msg::TrafficLightGroup tl_state;
 
+  if (planner_param_.v2i_use_rest_time) updateV2IRestTimeInfo();
+
   nearest_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
   for (const auto & scene_module : scene_modules_) {
     std::shared_ptr<TrafficLightModule> traffic_light_scene_module(
@@ -103,6 +122,9 @@ void TrafficLightModuleManager::launchNewModules(
       registerModule(std::make_shared<TrafficLightModule>(
         lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_,
         logger_.get_child("traffic_light_module"), clock_, time_keeper_,
+        std::bind(
+          &TrafficLightModuleManager::getV2IRestTimeToRedSignal, this,
+          traffic_light_reg_elem.first->id()),
         planning_factor_interface_));
       generateUUID(lane_id);
       updateRTCStatus(
@@ -163,6 +185,37 @@ bool TrafficLightModuleManager::hasSameTrafficLight(
   return false;
 }
 
+void TrafficLightModuleManager::updateV2IRestTimeInfo()
+{
+  if (!v2i_subscriber_) {
+    return;
+  }
+  auto msg = v2i_subscriber_->takeData();
+  if (!msg) {
+    return;
+  }
+
+  std::vector<lanelet::Id> traffic_light_ids;
+  traffic_light_id_to_rest_time_map_.clear();
+  for (const auto & car_light : msg->car_lights) {
+    for (const auto & state : car_light.states) {
+      traffic_light_id_to_rest_time_map_[state.traffic_light_group_id] =
+        car_light.min_rest_time_to_red;
+      traffic_light_ids.push_back(state.traffic_light_group_id);
+    }
+  }
+}
+
+std::optional<double> TrafficLightModuleManager::getV2IRestTimeToRedSignal(
+  const lanelet::Id & id) const
+{
+  const auto rest_time_to_red_signal = traffic_light_id_to_rest_time_map_.find(id);
+  if (rest_time_to_red_signal == traffic_light_id_to_rest_time_map_.end()) {
+    return std::nullopt;
+  }
+  return rest_time_to_red_signal->second;
+}
+
 }  // namespace autoware::behavior_velocity_planner
 
 #include <pluginlib/class_list_macros.hpp>
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
index 5ac32d1107880..242fb2f9468aa 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
@@ -20,11 +20,14 @@
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
 #include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <jpn_signal_v2i_msgs/msg/traffic_light_info.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <functional>
+#include <map>
 #include <memory>
 #include <optional>
 
@@ -55,6 +58,15 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
     const lanelet::TrafficLightConstPtr element,
     const lanelet::TrafficLightConstPtr registered_element) const;
 
+  void updateV2IRestTimeInfo();
+
+  std::optional<double> getV2IRestTimeToRedSignal(const lanelet::Id & id) const;
+
+  // V2I
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    jpn_signal_v2i_msgs::msg::TrafficLightInfo>::SharedPtr v2i_subscriber_;
+  std::map<lanelet::Id, double> traffic_light_id_to_rest_time_map_;
+
   // Debug
   rclcpp::Publisher<autoware_perception_msgs::msg::TrafficLightGroup>::SharedPtr pub_tl_state_;
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index 3a8692e9c53dd..af3ea8e3093ba 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -19,6 +19,7 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/traffic_light_utils/traffic_light_utils.hpp>
+#include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/distance.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
@@ -26,6 +27,7 @@
 #include <tf2/utils.h>
 
 #include <memory>
+#include <optional>
 
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_eigen/tf2_eigen.h>
@@ -45,6 +47,7 @@ TrafficLightModule::TrafficLightModule(
   lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock,
   const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
+  const std::function<std::optional<double>(void)> & get_rest_time_to_red_signal,
   const std::shared_ptr<planning_factor_interface::PlanningFactorInterface>
     planning_factor_interface)
 : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface),
@@ -53,7 +56,8 @@ TrafficLightModule::TrafficLightModule(
   lane_(lane),
   state_(State::APPROACH),
   debug_data_(),
-  is_prev_state_stop_(false)
+  is_prev_state_stop_(false),
+  get_rest_time_to_red_signal_(get_rest_time_to_red_signal)
 {
   planner_param_ = planner_param;
 }
@@ -107,6 +111,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path)
 
     first_ref_stop_path_point_index_ = stop_line.value().first;
 
+    // Use V2I if available
+    if (planner_param_.v2i_use_rest_time) {
+      std::optional<double> rest_time_to_red_signal = get_rest_time_to_red_signal_();
+      if (rest_time_to_red_signal.has_value()) {
+        const double rest_time_allowed_to_go_ahead =
+          rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass;
+        const double ego_v = planner_data_->current_velocity->twist.linear.x;
+
+        // Determine whether to stop based on velocity and time constraints
+        bool should_stop =
+          (ego_v >= planner_param_.v2i_velocity_threshold &&
+           ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) ||
+          (ego_v < planner_param_.v2i_velocity_threshold &&
+           rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure);
+
+        // RTC
+        setSafe(!should_stop);
+        if (isActivated()) {
+          return true;
+        }
+        *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second);
+        return true;
+      }
+      RCLCPP_WARN(
+        logger_, "Failed to get V2I rest time to red signal. traffic_light_lane_id: %ld", lane_id_);
+    }
+
     // Check if stop is coming.
     const bool is_stop_signal = isStopSignal();
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
index 71f0855a4af6f..9e668753e352c 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
@@ -15,6 +15,7 @@
 #ifndef SCENE_HPP_
 #define SCENE_HPP_
 
+#include <functional>
 #include <memory>
 #include <optional>
 #include <tuple>
@@ -63,6 +64,11 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
     double yellow_lamp_period;
     double stop_time_hysteresis;
     bool enable_pass_judge;
+    // V2I Parameter
+    bool v2i_use_rest_time;
+    double v2i_last_time_allowed_to_pass;
+    double v2i_velocity_threshold;
+    double v2i_required_time_to_departure;
   };
 
 public:
@@ -71,6 +77,7 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
     lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock,
     const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
+    const std::function<std::optional<double>(void)> & get_rest_time_to_red_signal,
     const std::shared_ptr<planning_factor_interface::PlanningFactorInterface>
       planning_factor_interface);
 
@@ -131,6 +138,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
 
   // Traffic Light State
   TrafficSignal looking_tl_state_;
+
+  // V2I
+  std::function<std::optional<double>(void)> get_rest_time_to_red_signal_;
 };
 }  // namespace autoware::behavior_velocity_planner
 

From 09837316dcb075b1f5cf35953b0de0a12d8bad25 Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Wed, 29 Jan 2025 17:59:23 +0900
Subject: [PATCH 03/10] add time stamp check

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../src/manager.cpp                           |  6 +-
 .../src/manager.hpp                           |  5 +-
 .../src/scene.cpp                             | 66 +++++++++++++------
 .../src/scene.hpp                             | 21 +++++-
 4 files changed, 70 insertions(+), 28 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
index a8279972d55f3..25f34b7e809d2 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
@@ -199,14 +199,14 @@ void TrafficLightModuleManager::updateV2IRestTimeInfo()
   traffic_light_id_to_rest_time_map_.clear();
   for (const auto & car_light : msg->car_lights) {
     for (const auto & state : car_light.states) {
-      traffic_light_id_to_rest_time_map_[state.traffic_light_group_id] =
-        car_light.min_rest_time_to_red;
+      traffic_light_id_to_rest_time_map_[state.traffic_light_group_id] = {
+        msg->header.stamp, car_light.min_rest_time_to_red};
       traffic_light_ids.push_back(state.traffic_light_group_id);
     }
   }
 }
 
-std::optional<double> TrafficLightModuleManager::getV2IRestTimeToRedSignal(
+std::optional<TrafficSignalTimeToRedStamped> TrafficLightModuleManager::getV2IRestTimeToRedSignal(
   const lanelet::Id & id) const
 {
   const auto rest_time_to_red_signal = traffic_light_id_to_rest_time_map_.find(id);
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
index 242fb2f9468aa..e2ecedfdf31de 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp
@@ -60,12 +60,13 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void updateV2IRestTimeInfo();
 
-  std::optional<double> getV2IRestTimeToRedSignal(const lanelet::Id & id) const;
+  std::optional<TrafficSignalTimeToRedStamped> getV2IRestTimeToRedSignal(
+    const lanelet::Id & id) const;
 
   // V2I
   autoware::universe_utils::InterProcessPollingSubscriber<
     jpn_signal_v2i_msgs::msg::TrafficLightInfo>::SharedPtr v2i_subscriber_;
-  std::map<lanelet::Id, double> traffic_light_id_to_rest_time_map_;
+  std::map<lanelet::Id, TrafficSignalTimeToRedStamped> traffic_light_id_to_rest_time_map_;
 
   // Debug
   rclcpp::Publisher<autoware_perception_msgs::msg::TrafficLightGroup>::SharedPtr pub_tl_state_;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index af3ea8e3093ba..ee949c14366aa 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -19,6 +19,7 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/traffic_light_utils/traffic_light_utils.hpp>
+#include <rclcpp/clock.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/distance.hpp>
@@ -26,6 +27,7 @@
 
 #include <tf2/utils.h>
 
+#include <ctime>
 #include <memory>
 #include <optional>
 
@@ -47,7 +49,8 @@ TrafficLightModule::TrafficLightModule(
   lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock,
   const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
-  const std::function<std::optional<double>(void)> & get_rest_time_to_red_signal,
+  const std::function<std::optional<TrafficSignalTimeToRedStamped>(void)> &
+    get_rest_time_to_red_signal,
   const std::shared_ptr<planning_factor_interface::PlanningFactorInterface>
     planning_factor_interface)
 : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface),
@@ -113,29 +116,12 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path)
 
     // Use V2I if available
     if (planner_param_.v2i_use_rest_time) {
-      std::optional<double> rest_time_to_red_signal = get_rest_time_to_red_signal_();
-      if (rest_time_to_red_signal.has_value()) {
-        const double rest_time_allowed_to_go_ahead =
-          rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass;
-        const double ego_v = planner_data_->current_velocity->twist.linear.x;
-
-        // Determine whether to stop based on velocity and time constraints
-        bool should_stop =
-          (ego_v >= planner_param_.v2i_velocity_threshold &&
-           ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) ||
-          (ego_v < planner_param_.v2i_velocity_threshold &&
-           rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure);
-
-        // RTC
-        setSafe(!should_stop);
-        if (isActivated()) {
-          return true;
-        }
+      bool is_v2i_handled = handleV2I(signed_arc_length_to_stop_point, [&]() {
         *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second);
+      });
+      if (is_v2i_handled) {
         return true;
       }
-      RCLCPP_WARN(
-        logger_, "Failed to get V2I rest time to red signal. traffic_light_lane_id: %ld", lane_id_);
     }
 
     // Check if stop is coming.
@@ -333,4 +319,42 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose(
   return modified_path;
 }
 
+bool TrafficLightModule::handleV2I(
+  const double & signed_arc_length_to_stop_point, const std::function<void()> & insert_stop_pose)
+{
+  std::optional<TrafficSignalTimeToRedStamped> rest_time_to_red_signal =
+    get_rest_time_to_red_signal_();
+
+  if (!rest_time_to_red_signal) {
+    RCLCPP_WARN_THROTTLE(
+      logger_, *clock_, 5000,
+      "Failed to get V2I rest time to red signal. traffic_light_lane_id: %ld", lane_id_);
+    return false;
+  }
+
+  auto time_diff = (clock_->now() - rest_time_to_red_signal->stamp).seconds();
+  if (time_diff > planner_param_.tl_state_timeout) {
+    RCLCPP_WARN_THROTTLE(
+      logger_, *clock_, 5000, "V2I data is timeout. traffic_light_lane_id: %ld, time diff: %f",
+      lane_id_, time_diff);
+    return false;
+  }
+
+  const double rest_time_allowed_to_go_ahead =
+    rest_time_to_red_signal->time_to_red - planner_param_.v2i_last_time_allowed_to_pass;
+  const double ego_v = planner_data_->current_velocity->twist.linear.x;
+
+  const bool should_stop =
+    (ego_v >= planner_param_.v2i_velocity_threshold &&
+     ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) ||
+    (ego_v < planner_param_.v2i_velocity_threshold &&
+     rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure);
+
+  setSafe(!should_stop);
+  if (isActivated()) {
+    return true;
+  }
+  insert_stop_pose();
+  return true;
+}
 }  // namespace autoware::behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
index 9e668753e352c..c1577a95f6536 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp
@@ -34,6 +34,13 @@
 
 namespace autoware::behavior_velocity_planner
 {
+
+struct TrafficSignalTimeToRedStamped
+{
+  builtin_interfaces::msg::Time stamp;
+  double time_to_red{};
+};
+
 class TrafficLightModule : public SceneModuleInterfaceWithRTC
 {
 public:
@@ -77,7 +84,8 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
     lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock,
     const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
-    const std::function<std::optional<double>(void)> & get_rest_time_to_red_signal,
+    const std::function<std::optional<TrafficSignalTimeToRedStamped>(void)> &
+      get_rest_time_to_red_signal,
     const std::shared_ptr<planning_factor_interface::PlanningFactorInterface>
       planning_factor_interface);
 
@@ -110,6 +118,15 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
 
   void updateTrafficSignal();
 
+  /**
+   * @brief Handle V2I Rest Time to Red Signal
+   * @param signed_arc_length_to_stop_point signed arc length to stop point
+   * @param output_path output path
+   * @return true if V2I is handled
+   */
+  bool handleV2I(
+    const double & signed_arc_length_to_stop_point, const std::function<void()> & insert_stop_pose);
+
   // Lane id
   const int64_t lane_id_;
 
@@ -140,7 +157,7 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC
   TrafficSignal looking_tl_state_;
 
   // V2I
-  std::function<std::optional<double>(void)> get_rest_time_to_red_signal_;
+  std::function<std::optional<TrafficSignalTimeToRedStamped>(void)> get_rest_time_to_red_signal_;
 };
 }  // namespace autoware::behavior_velocity_planner
 

From 77ac5e339e919ae8192ae481ecdf2878e6adc3be Mon Sep 17 00:00:00 2001
From: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Date: Wed, 22 Jan 2025 18:52:38 +0900
Subject: [PATCH 04/10] chore(sync-files.yaml): not synchronize
 `github-release.yaml` (#1776)

not sync github-release

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
---
 .github/sync-files.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
index 4c09e072766fa..8afb7671215f4 100644
--- a/.github/sync-files.yaml
+++ b/.github/sync-files.yaml
@@ -15,7 +15,6 @@
     - source: .github/workflows/comment-on-pr.yaml
     - source: .github/workflows/delete-closed-pr-docs.yaml
     - source: .github/workflows/deploy-docs.yaml
-    - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml
     - source: .github/workflows/pre-commit-optional.yaml
     - source: .github/workflows/pre-commit-optional-autoupdate.yaml

From b544bf765ee4b6475692ec96fd468d57807c7d4f Mon Sep 17 00:00:00 2001
From: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Date: Wed, 22 Jan 2025 18:52:38 +0900
Subject: [PATCH 05/10] chore(sync-files.yaml): not synchronize
 `github-release.yaml` (#1776)

not sync github-release

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
---
 .github/sync-files.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
index 4c09e072766fa..8afb7671215f4 100644
--- a/.github/sync-files.yaml
+++ b/.github/sync-files.yaml
@@ -15,7 +15,6 @@
     - source: .github/workflows/comment-on-pr.yaml
     - source: .github/workflows/delete-closed-pr-docs.yaml
     - source: .github/workflows/deploy-docs.yaml
-    - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml
     - source: .github/workflows/pre-commit-optional.yaml
     - source: .github/workflows/pre-commit-optional-autoupdate.yaml

From 201e06bed1626a67f9487b11a3b16c5bf9276001 Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Thu, 30 Jan 2025 20:35:25 +0900
Subject: [PATCH 06/10] fix(behavior_velocity_traffic_light): stop when the
 signal is timed out

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../src/scene.cpp                                         | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index ee949c14366aa..7c26af75fbec6 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -114,8 +114,11 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path)
 
     first_ref_stop_path_point_index_ = stop_line.value().first;
 
+    // Check if stop is coming.
+    const bool is_stop_signal = isStopSignal();
+
     // Use V2I if available
-    if (planner_param_.v2i_use_rest_time) {
+    if (planner_param_.v2i_use_rest_time && !is_stop_signal) {
       bool is_v2i_handled = handleV2I(signed_arc_length_to_stop_point, [&]() {
         *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second);
       });
@@ -124,9 +127,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path)
       }
     }
 
-    // Check if stop is coming.
-    const bool is_stop_signal = isStopSignal();
-
     // Update stop signal received time
     if (is_stop_signal) {
       if (!stop_signal_received_time_ptr_) {

From 6f8695fcadcdcb2ecae3c102d19942b93433252e Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Thu, 30 Jan 2025 20:35:25 +0900
Subject: [PATCH 07/10] fix(behavior_velocity_traffic_light): set
 prev_state_stop when using time to red signal

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../src/scene.cpp                                               | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index 7c26af75fbec6..feb65d3d65d47 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -352,9 +352,11 @@ bool TrafficLightModule::handleV2I(
 
   setSafe(!should_stop);
   if (isActivated()) {
+    is_prev_state_stop_ = false;
     return true;
   }
   insert_stop_pose();
+  is_prev_state_stop_ = true;
   return true;
 }
 }  // namespace autoware::behavior_velocity_planner

From 524986080b7519ffa0b04bf4747ac9adcd75fe16 Mon Sep 17 00:00:00 2001
From: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Date: Wed, 22 Jan 2025 18:52:38 +0900
Subject: [PATCH 08/10] chore(sync-files.yaml): not synchronize
 `github-release.yaml` (#1776)

not sync github-release

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
---
 .github/sync-files.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
index 4c09e072766fa..8afb7671215f4 100644
--- a/.github/sync-files.yaml
+++ b/.github/sync-files.yaml
@@ -15,7 +15,6 @@
     - source: .github/workflows/comment-on-pr.yaml
     - source: .github/workflows/delete-closed-pr-docs.yaml
     - source: .github/workflows/deploy-docs.yaml
-    - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml
     - source: .github/workflows/pre-commit-optional.yaml
     - source: .github/workflows/pre-commit-optional-autoupdate.yaml

From e5171d7eea1389c7d503050efa01351f5a4d8526 Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Fri, 31 Jan 2025 17:47:12 +0900
Subject: [PATCH 09/10] fix(bvp): traffic light state debug This commit
 corresponds to https://github.com/tier4/autoware.universe/pull/1083.

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../src/scene.cpp                                              | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index feb65d3d65d47..3a7ed28699058 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -203,6 +203,9 @@ void TrafficLightModule::updateTrafficSignal()
   TrafficSignalStamped signal;
   if (!findValidTrafficSignal(signal)) {
     // Don't stop if it never receives traffic light topic.
+    // Reset looking_tl_state
+    looking_tl_state_.elements.clear();
+    looking_tl_state_.traffic_light_group_id = 0;
     return;
   }
 

From d0fcee891f7a962e54ab6935d0c71b36a4d5b2fe Mon Sep 17 00:00:00 2001
From: "Y.Hisaki" <yhisaki31@gmail.com>
Date: Fri, 31 Jan 2025 18:23:22 +0900
Subject: [PATCH 10/10] feat(behavior_velocity_traffic_light_module): change
 threshold velocity of pass judge for preventing sudden stop

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../src/scene.cpp                                               | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
index 3a7ed28699058..78eb824603340 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp
@@ -233,7 +233,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
     delay_response_time);
 
   const bool distance_stoppable = pass_judge_line_distance < signed_arc_length;
-  const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0;
+  const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 1.0;
   const bool stoppable = distance_stoppable || slow_velocity;
   const bool reachable = signed_arc_length < reachable_distance;