feat(autoware_traffic_light_arbiter): add current time validation

perception/autoware_traffic_light_arbiter/README.md

@@ -41,9 +41,10 @@ The table below outlines how the matching process determines the output based on
 
 ### Core Parameters
 
-| Name                        | Type   | Default Value | Description                                                                                                                                                                    |
-| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging                                                                                                    |
-| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging                                                                                                   |
-| `external_priority`         | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria                                               |
-| `enable_signal_matching`    | bool   | false         | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |
+| Name                              | Type   | Default Value | Description                                                                                                                                                                    |
+| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| `external_current_time_tolerance` | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with current time                                                                    |
+| `external_time_tolerance`         | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message                                               |
+| `perception_time_tolerance`       | double | 1.0           | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message                                                |
+| `external_priority`               | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria                                               |
+| `enable_signal_matching`          | bool   | false         | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |

perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    external_current_time_tolerance: 5.0
     external_time_tolerance: 5.0
     perception_time_tolerance: 1.0
     external_priority: false

perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp

@@ -51,6 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node
 
   std::unique_ptr<std::unordered_set<lanelet::Id>> map_regulatory_elements_set_;
 
+  double external_current_time_tolerance_;
   double external_time_tolerance_;
   double perception_time_tolerance_;
   bool external_priority_;

perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 The Autoware Contributors
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -70,6 +70,8 @@
 TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
 : Node("traffic_light_arbiter", options)
 {
+  external_current_time_tolerance_ =
+    this->declare_parameter<double>("external_current_time_tolerance", 5.0);
   external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance", 5.0);
   perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance", 1.0);
   external_priority_ = this->declare_parameter<bool>("external_priority", false);
@@ -129,6 +131,12 @@
 
 void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg)
 {
+  if ((this->now() - rclcpp::Time(msg->stamp)).seconds() > external_current_time_tolerance_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages");
+    return;
+  }
+
   latest_external_msg_ = *msg;
 
   if (
@@ -229,6 +237,13 @@
   }
 
   pub_->publish(output_signals_msg);
+
+  const auto latest_time =
+    std::max(rclcpp::Time(latest_perception_msg_.stamp), rclcpp::Time(latest_external_msg_.stamp));
+  if (rclcpp::Time(output_signals_msg.stamp) < latest_time) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Published traffic signal messages are not latest");
+  }
 }
 }  // namespace autoware