diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 619154e1e183b..f27afab62818e 100644 --- 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 index dfe12ff352f16..36e1b8593bebc 100644 --- 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/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp index ccd928d52b367..916bd04a6bdd0 100644 --- a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/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> 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 index e71629fa5dd28..8ce002780813f 100644 --- 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 #include +#include #include #include #include @@ -70,6 +71,7 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { + external_delay_tolerance_ = this->declare_parameter("external_delay_tolerance", 5.0); external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0); perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0); external_priority_ = this->declare_parameter("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 index f993ad7cec84d..44b3ca7925fd0 100644 --- 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(output_topic, callback); + // stamp preparation + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg(