feat(autoware_traffic_light_arbiter): add current time validation (au…

…towarefoundation#9747) * add current time validation Signed-off-by: MasatoSaeki <[email protected]> * style(pre-commit): autofix * change ros parameter name Signed-off-by: MasatoSaeki <[email protected]> * style(pre-commit): autofix * add validation with absolute function Signed-off-by: MasatoSaeki <[email protected]> * add timestamp of topic in test Signed-off-by: MasatoSaeki <[email protected]> * fix ci error Signed-off-by: MasatoSaeki <[email protected]> --------- Signed-off-by: MasatoSaeki <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
kminoda · Dec 25, 2024 · 8673ced · 8673ced
1 parent 5100b66
commit 8673ced
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 4 deletions.
diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_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 |
diff --git a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/autoware_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

diff --git a/...re_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp b/...re_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_delay_tolerance_;
   double external_time_tolerance_;
   double perception_time_tolerance_;
   bool external_priority_;

diff --git a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_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>
@@ -70,6 +71,7 @@ namespace autoware
 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);
@@ -119,7 +121,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();
   }
@@ -129,10 +131,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();
   }
@@ -229,6 +237,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");
+  }
 }
 }  // namespace autoware
 

diff --git a/perception/autoware_traffic_light_arbiter/test/test_node.cpp b/perception/autoware_traffic_light_arbiter/test/test_node.cpp
@@ -196,6 +196,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyPerceptionMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(