feat(autoware_traffic_light_arbiter): add current time validation (#9…

perception/traffic_light_arbiter/README.md

@@ -43,7 +43,8 @@ The table below outlines how the matching process determines the output based on
 
 | 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_delay_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/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml

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

perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp

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

perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp

@@ -20,6 +20,7 @@
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
+#include <algorithm>
 #include <map>
 #include <memory>
 #include <tuple>
@@ -68,6 +69,7 @@ std::vector<TrafficLightConstPtr> filter_pedestrian_signals(const LaneletMapCons
 TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
 : Node("traffic_light_arbiter", options)
 {
+  external_delay_tolerance_ = this->declare_parameter<double>("external_delay_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);
@@ -117,7 +119,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
   latest_perception_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) >
     external_time_tolerance_) {
     latest_external_msg_.traffic_light_groups.clear();
   }
@@ -127,10 +129,16 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
 
 void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg)
 {
+  if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages");
+    return;
+  }
+
   latest_external_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) >
     perception_time_tolerance_) {
     latest_perception_msg_.traffic_light_groups.clear();
   }
@@ -227,6 +235,13 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim
   }
 
   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");
+  }
 }
 
 #include <rclcpp_components/register_node_macro.hpp>