From 403282bd80e900b6c6677e6bc89eea26acefa173 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Thu, 27 Jul 2023 22:21:12 +0900 Subject: [PATCH 1/4] fix(traffic_light_arbiter): publish traffic light info even when a map without traffic lights is subscribed Signed-off-by: tomoya.kimura --- .../traffic_light_arbiter/traffic_light_arbiter.hpp | 3 ++- .../traffic_light_arbiter/src/traffic_light_arbiter.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index afb5984c288fc..886b8bde08aa4 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -22,6 +22,7 @@ #include +#include #include class TrafficLightArbiter : public rclcpp::Node @@ -45,7 +46,7 @@ class TrafficLightArbiter : public rclcpp::Node void onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg); void arbitrateAndPublish(const builtin_interfaces::msg::Time & stamp); - std::unordered_set map_regulatory_elements_set_; + std::shared_ptr> map_regulatory_elements_set_; double external_time_tolerance_; double perception_time_tolerance_; diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index eb940fc18c32d..b0a5fad966390 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -72,9 +72,10 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - map_regulatory_elements_set_.clear(); + map_regulatory_elements_set_ = std::make_shared>(); + for (const auto & signal : signals) { - map_regulatory_elements_set_.emplace(signal->id()); + map_regulatory_elements_set_->emplace(signal->id()); } } @@ -109,14 +110,14 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim using ElementAndPriority = std::pair; std::unordered_map> regulatory_element_signals_map; - if (map_regulatory_elements_set_.empty()) { + if (map_regulatory_elements_set_ == nullptr) { RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); return; } auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; - if (!map_regulatory_elements_set_.count(id)) { + if (!map_regulatory_elements_set_->count(id)) { RCLCPP_WARN( get_logger(), "Received a traffic signal not present in the current map (%lu)", id); return; From 9b69d4d7a7259c3c0bd5639c898c87a2f4f36c86 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Thu, 27 Jul 2023 22:28:41 +0900 Subject: [PATCH 2/4] feat: publish empty message if the size of elemsents_set is 0 Signed-off-by: tomoya.kimura --- .../src/traffic_light_arbiter.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index b0a5fad966390..bbea3af351bdb 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -115,6 +115,14 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim return; } + TrafficSignalArray output_signals_msg; + output_signals_msg.stamp = stamp; + + if (map_regulatory_elements_set_->empty()) { + pub_->publish(output_signals_msg); + return; + } + auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; if (!map_regulatory_elements_set_->count(id)) { @@ -166,8 +174,6 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim return highest_score_elements_vector; }; - TrafficSignalArray output_signals_msg; - output_signals_msg.stamp = stamp; output_signals_msg.signals.reserve(regulatory_element_signals_map.size()); for (const auto & [regulatory_element_id, elements] : regulatory_element_signals_map) { From 5fa3c4b1936d8ed2d7211aef56aef7331bbb6190 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 27 Jul 2023 23:11:47 +0900 Subject: [PATCH 3/4] Update perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp Co-authored-by: Kenzo Lobos Tsunekawa --- .../include/traffic_light_arbiter/traffic_light_arbiter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index 886b8bde08aa4..485ce84c5fea6 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -46,7 +46,7 @@ class TrafficLightArbiter : public rclcpp::Node void onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg); void arbitrateAndPublish(const builtin_interfaces::msg::Time & stamp); - std::shared_ptr> map_regulatory_elements_set_; + std::unique_ptr> map_regulatory_elements_set_; double external_time_tolerance_; double perception_time_tolerance_; From cbceb8eb0cadbceb58e41cf07ff635526c5bf14f Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 27 Jul 2023 23:11:56 +0900 Subject: [PATCH 4/4] Update perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp Co-authored-by: Kenzo Lobos Tsunekawa --- perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index bbea3af351bdb..dfa01878bb81f 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -72,7 +72,7 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - map_regulatory_elements_set_ = std::make_shared>(); + map_regulatory_elements_set_ = std::make_unique>(); for (const auto & signal : signals) { map_regulatory_elements_set_->emplace(signal->id());