add time stamp check

Signed-off-by: Y.Hisaki <[email protected]>

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);

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_;

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp

@@ -19,13 +19,15 @@
 #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>
 #include <boost/geometry/algorithms/intersection.hpp>
 
 #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

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