From 823156937911873780851e0d95f6df6f2e86d3e2 Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 24 Dec 2024 08:37:16 +0900 Subject: [PATCH 1/7] add current time validation Signed-off-by: MasatoSaeki --- .../autoware_traffic_light_arbiter/README.md | 9 +++++---- .../config/traffic_light_arbiter.param.yaml | 1 + .../traffic_light_arbiter.hpp | 1 + .../src/traffic_light_arbiter.cpp | 14 ++++++++++++++ 4 files changed, 21 insertions(+), 4 deletions(-) diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 619154e1e183b..47f623ce84941 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_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 | +| `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 | 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..16498d602c57c 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_current_time_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..24675c34f009c 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_current_time_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..e5963c9530b83 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -70,6 +70,7 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { + external_current_time_tolerance_ = this->declare_parameter("external_current_time_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); @@ -129,6 +130,12 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP 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 +236,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 From ac94a44576f423271a772fb68e0d8b46da9d2b23 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 23 Dec 2024 23:45:58 +0000 Subject: [PATCH 2/7] style(pre-commit): autofix --- .../autoware_traffic_light_arbiter/README.md | 14 +++++++------- .../src/traffic_light_arbiter.cpp | 9 +++++---- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 47f623ce84941..2c38df96fcef4 100644 --- a/perception/autoware_traffic_light_arbiter/README.md +++ b/perception/autoware_traffic_light_arbiter/README.md @@ -41,10 +41,10 @@ The table below outlines how the matching process determines the output based on ### Core Parameters -| 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 | +| 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 | 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 e5963c9530b83..335391b29c864 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -70,7 +70,8 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { - external_current_time_tolerance_ = this->declare_parameter("external_current_time_tolerance", 5.0); + external_current_time_tolerance_ = + this->declare_parameter("external_current_time_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); @@ -130,7 +131,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg) { - if ((this->now() - rclcpp::Time(msg->stamp)).seconds() > external_current_time_tolerance_) { + 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; @@ -237,8 +238,8 @@ 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)); + 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"); From 227d53926f6802fe4532ca53b217e2205957cf85 Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 24 Dec 2024 10:56:42 +0900 Subject: [PATCH 3/7] change ros parameter name Signed-off-by: MasatoSaeki --- perception/autoware_traffic_light_arbiter/README.md | 2 +- .../config/traffic_light_arbiter.param.yaml | 2 +- .../traffic_light_arbiter/traffic_light_arbiter.hpp | 2 +- .../src/traffic_light_arbiter.cpp | 6 +++--- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 2c38df96fcef4..b9ede05057658 100644 --- a/perception/autoware_traffic_light_arbiter/README.md +++ b/perception/autoware_traffic_light_arbiter/README.md @@ -43,7 +43,7 @@ The table below outlines how the matching process determines the output based on | 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_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 | 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 16498d602c57c..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,6 +1,6 @@ /**: ros__parameters: - external_current_time_tolerance: 5.0 + 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 24675c34f009c..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,7 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node std::unique_ptr> map_regulatory_elements_set_; - double external_current_time_tolerance_; + 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 335391b29c864..3a5345eca47ce 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -70,8 +70,8 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { - external_current_time_tolerance_ = - this->declare_parameter("external_current_time_tolerance", 5.0); + 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); @@ -131,7 +131,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg) { - if ((this->now() - rclcpp::Time(msg->stamp)).seconds() > external_current_time_tolerance_) { + if ((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; From ab2e42815e546f0da63ef5d0a1d9636f9ec0cbfa Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 24 Dec 2024 02:00:25 +0000 Subject: [PATCH 4/7] style(pre-commit): autofix --- .../autoware_traffic_light_arbiter/README.md | 14 +++++++------- .../src/traffic_light_arbiter.cpp | 3 +-- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index b9ede05057658..f27afab62818e 100644 --- a/perception/autoware_traffic_light_arbiter/README.md +++ b/perception/autoware_traffic_light_arbiter/README.md @@ -41,10 +41,10 @@ The table below outlines how the matching process determines the output based on ### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `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 | +| Name | Type | Default Value | Description | +| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `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/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3a5345eca47ce..2e820904f9e00 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -70,8 +70,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_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); From 982643f04f3ae42bfaca6d5c102601ce71a6d08f Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 24 Dec 2024 15:08:45 +0900 Subject: [PATCH 5/7] add validation with absolute function Signed-off-by: MasatoSaeki --- .../src/traffic_light_arbiter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 2e820904f9e00..3f3d969164ab4 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -120,7 +120,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(); } @@ -130,7 +130,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg) { - if ((this->now() - rclcpp::Time(msg->stamp)).seconds() > external_delay_tolerance_) { + 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; @@ -139,7 +139,7 @@ void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr 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(); } From e2916dddee7b6afcbfeafd86a6a31706108b2233 Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 24 Dec 2024 15:27:17 +0900 Subject: [PATCH 6/7] add timestamp of topic in test Signed-off-by: MasatoSaeki --- .../autoware_traffic_light_arbiter/test/test_node.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) 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( From 0e85287308753ca06275ffec78bff781ba732271 Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 24 Dec 2024 15:45:28 +0900 Subject: [PATCH 7/7] fix ci error Signed-off-by: MasatoSaeki --- .../autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp | 1 + 1 file changed, 1 insertion(+) 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 3f3d969164ab4..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