From a1c22bd09215ab8d196dfe230e761d19374f9411 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Feb 2023 10:41:24 +0900 Subject: [PATCH 01/10] feat: add package Signed-off-by: Takagi, Isamu --- .../traffic_light_selector/CMakeLists.txt | 15 ++++ .../launch/traffic_light_selector.launch.xml | 9 +++ perception/traffic_light_selector/package.xml | 28 +++++++ .../src/traffic_light_selector.cpp | 81 +++++++++++++++++++ .../src/traffic_light_selector.hpp | 46 +++++++++++ 5 files changed, 179 insertions(+) create mode 100644 perception/traffic_light_selector/CMakeLists.txt create mode 100644 perception/traffic_light_selector/launch/traffic_light_selector.launch.xml create mode 100644 perception/traffic_light_selector/package.xml create mode 100644 perception/traffic_light_selector/src/traffic_light_selector.cpp create mode 100644 perception/traffic_light_selector/src/traffic_light_selector.hpp diff --git a/perception/traffic_light_selector/CMakeLists.txt b/perception/traffic_light_selector/CMakeLists.txt new file mode 100644 index 0000000000000..39b9d9b4c0f73 --- /dev/null +++ b/perception/traffic_light_selector/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(traffic_light_selector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_selector.cpp +) + +rclcpp_components_register_nodes(${PROJECT_NAME} + TrafficLightSelector +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml new file mode 100644 index 0000000000000..86817161bcda1 --- /dev/null +++ b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/perception/traffic_light_selector/package.xml b/perception/traffic_light_selector/package.xml new file mode 100644 index 0000000000000..62374ddb7c0bf --- /dev/null +++ b/perception/traffic_light_selector/package.xml @@ -0,0 +1,28 @@ + + + + traffic_light_selector + 0.1.0 + The traffic_light_selector package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_perception_msgs + lanelet2_core + lanelet2_extension + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp new file mode 100644 index 0000000000000..f4f70403b1c8c --- /dev/null +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_light_selector.hpp" + +#include + +#include + +#include +#include + +namespace lanelet +{ + +using TrafficLightConstPtr = std::shared_ptr; + +std::vector filter_traffic_signals(const LaneletMapConstPtr map) +{ + std::vector signals; + for (const auto & element : map->regulatoryElementLayer) { + const auto signal = std::dynamic_pointer_cast(element); + if (signal) { + signals.push_back(signal); + } + } + return signals; +} + +} // namespace lanelet + +TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) +: Node("traffic_light_selector", options) +{ + sub_map_ = create_subscription( + "~/sub/vector_map", rclcpp::QoS(1).transient_local(), + std::bind(&TrafficLightSelector::on_map, this, std::placeholders::_1)); + sub_lights_ = create_subscription( + "~/sub/traffic_lights", rclcpp::QoS(1), + std::bind(&TrafficLightSelector::on_lights, this, std::placeholders::_1)); + pub_signals_ = create_publisher("~/pub/traffic_signals", rclcpp::QoS(1)); +} + +void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) +{ + const auto map = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map); + + const auto signals = lanelet::filter_traffic_signals(map); + light_to_signal_.clear(); + for (const auto & signal : signals) { + for (const auto & light : signal->trafficLights()) { + light_to_signal_[light.id()] = signal->id(); + } + } +} + +void TrafficLightSelector::on_lights(const TrafficLightArray::ConstSharedPtr msg) +{ + for (const auto & light : msg->signals) { + RCLCPP_INFO_STREAM(get_logger(), "ID: " << light.map_primitive_id); + for (const auto & e : light.lights) { + RCLCPP_INFO_STREAM( + get_logger(), " - " << (int)e.color << " " << (int)e.shape << " " << e.confidence); + } + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightSelector) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.hpp b/perception/traffic_light_selector/src/traffic_light_selector.hpp new file mode 100644 index 0000000000000..ff4dbbfa8ec43 --- /dev/null +++ b/perception/traffic_light_selector/src/traffic_light_selector.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_LIGHT_SELECTOR_HPP_ +#define TRAFFIC_LIGHT_SELECTOR_HPP_ + +#include + +#include +#include +#include + +#include + +#include + +class TrafficLightSelector : public rclcpp::Node +{ +public: + explicit TrafficLightSelector(const rclcpp::NodeOptions & options); + +private: + using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using TrafficLightArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_lights_; + rclcpp::Publisher::SharedPtr pub_signals_; + void on_map(const LaneletMapBin::ConstSharedPtr msg); + void on_lights(const TrafficLightArray::ConstSharedPtr msg); + + std::unordered_map light_to_signal_; +}; + +#endif // TRAFFIC_LIGHT_SELECTOR_HPP_ From cba093d3406208ae0783fd2de65fafc67c744254 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 1 Feb 2023 14:29:15 +0900 Subject: [PATCH 02/10] feat: publish signals Signed-off-by: Takagi, Isamu --- .../src/traffic_light_selector.cpp | 69 +++++++++++++++++-- 1 file changed, 65 insertions(+), 4 deletions(-) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index f4f70403b1c8c..127ae075d8a87 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -40,6 +40,54 @@ std::vector filter_traffic_signals(const LaneletMapConstPt } // namespace lanelet +namespace utils +{ + +using Input = autoware_auto_perception_msgs::msg::TrafficLight; +using Output = autoware_perception_msgs::msg::TrafficLightElement; + +template +V at_or(const std::unordered_map & map, const K & key, const V & value) +{ + return map.count(key) ? map.at(key) : value; +} + +Output convert(const Input & input) +{ + // clang-format off + static const std::unordered_map color_map({ + {Input::RED, Output::RED}, + {Input::AMBER, Output::AMBER}, + {Input::GREEN, Output::GREEN}, + {Input::WHITE, Output::WHITE} + }); + static const std::unordered_map shape_map({ + {Input::CIRCLE, Output::CIRCLE}, + {Input::LEFT_ARROW, Output::LEFT_ARROW}, + {Input::RIGHT_ARROW, Output::RIGHT_ARROW}, + {Input::UP_ARROW, Output::UP_ARROW}, + {Input::DOWN_ARROW, Output::DOWN_ARROW}, + {Input::DOWN_LEFT_ARROW, Output::DOWN_LEFT_ARROW}, + {Input::DOWN_RIGHT_ARROW, Output::DOWN_RIGHT_ARROW}, + {Input::CROSS, Output::CROSS} + }); + static const std::unordered_map status_map({ + {Input::SOLID_OFF, Output::SOLID_OFF}, + {Input::SOLID_ON, Output::SOLID_ON}, + {Input::FLASHING, Output::FLASHING} + }); + // clang-format on + + Output output; + output.color = at_or(color_map, input.color, Output::UNKNOWN); + output.shape = at_or(shape_map, input.shape, Output::UNKNOWN); + output.color = at_or(status_map, input.status, Output::UNKNOWN); + output.confidence = input.confidence; + return output; +} + +} // namespace utils + TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) : Node("traffic_light_selector", options) { @@ -68,13 +116,26 @@ void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) void TrafficLightSelector::on_lights(const TrafficLightArray::ConstSharedPtr msg) { + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement; + + std::unordered_map> elements; for (const auto & light : msg->signals) { - RCLCPP_INFO_STREAM(get_logger(), "ID: " << light.map_primitive_id); - for (const auto & e : light.lights) { - RCLCPP_INFO_STREAM( - get_logger(), " - " << (int)e.color << " " << (int)e.shape << " " << e.confidence); + const auto id = light_to_signal_[light.map_primitive_id]; + for (const auto & element : light.lights) { + elements[id].push_back(utils::convert(element)); } } + + TrafficSignalArray array; + array.stamp = now(); + for (const auto & [id, elements] : elements) { + TrafficSignal signal; + signal.traffic_signal_id = id; + signal.elements = elements; + array.signals.push_back(signal); + } + pub_signals_->publish(array); } #include From 26392969028836bbf32081794b40941719f7e165 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 16 Feb 2023 18:31:51 +0900 Subject: [PATCH 03/10] feat: add v2x input Signed-off-by: Takagi, Isamu --- .../src/traffic_light_selector.cpp | 92 +++++++++++++------ .../src/traffic_light_selector.hpp | 17 +++- 2 files changed, 76 insertions(+), 33 deletions(-) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index 127ae075d8a87..ec07cc597ccc8 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -43,8 +43,10 @@ std::vector filter_traffic_signals(const LaneletMapConstPt namespace utils { -using Input = autoware_auto_perception_msgs::msg::TrafficLight; -using Output = autoware_perception_msgs::msg::TrafficLightElement; +using OldData = autoware_auto_perception_msgs::msg::TrafficSignal; +using OldElem = autoware_auto_perception_msgs::msg::TrafficLight; +using NewData = autoware_perception_msgs::msg::TrafficLight; +using NewElem = autoware_perception_msgs::msg::TrafficLightElement; template V at_or(const std::unordered_map & map, const K & key, const V & value) @@ -52,40 +54,50 @@ V at_or(const std::unordered_map & map, const K & key, const V & value) return map.count(key) ? map.at(key) : value; } -Output convert(const Input & input) +NewElem convert(const OldElem & input) { // clang-format off - static const std::unordered_map color_map({ - {Input::RED, Output::RED}, - {Input::AMBER, Output::AMBER}, - {Input::GREEN, Output::GREEN}, - {Input::WHITE, Output::WHITE} + static const std::unordered_map color_map({ + {OldElem::RED, NewElem::RED}, + {OldElem::AMBER, NewElem::AMBER}, + {OldElem::GREEN, NewElem::GREEN}, + {OldElem::WHITE, NewElem::WHITE} }); - static const std::unordered_map shape_map({ - {Input::CIRCLE, Output::CIRCLE}, - {Input::LEFT_ARROW, Output::LEFT_ARROW}, - {Input::RIGHT_ARROW, Output::RIGHT_ARROW}, - {Input::UP_ARROW, Output::UP_ARROW}, - {Input::DOWN_ARROW, Output::DOWN_ARROW}, - {Input::DOWN_LEFT_ARROW, Output::DOWN_LEFT_ARROW}, - {Input::DOWN_RIGHT_ARROW, Output::DOWN_RIGHT_ARROW}, - {Input::CROSS, Output::CROSS} + static const std::unordered_map shape_map({ + {OldElem::CIRCLE, NewElem::CIRCLE}, + {OldElem::LEFT_ARROW, NewElem::LEFT_ARROW}, + {OldElem::RIGHT_ARROW, NewElem::RIGHT_ARROW}, + {OldElem::UP_ARROW, NewElem::UP_ARROW}, + {OldElem::DOWN_ARROW, NewElem::DOWN_ARROW}, + {OldElem::DOWN_LEFT_ARROW, NewElem::DOWN_LEFT_ARROW}, + {OldElem::DOWN_RIGHT_ARROW, NewElem::DOWN_RIGHT_ARROW}, + {OldElem::CROSS, NewElem::CROSS} }); - static const std::unordered_map status_map({ - {Input::SOLID_OFF, Output::SOLID_OFF}, - {Input::SOLID_ON, Output::SOLID_ON}, - {Input::FLASHING, Output::FLASHING} + static const std::unordered_map status_map({ + {OldElem::SOLID_OFF, NewElem::SOLID_OFF}, + {OldElem::SOLID_ON, NewElem::SOLID_ON}, + {OldElem::FLASHING, NewElem::FLASHING} }); // clang-format on - Output output; - output.color = at_or(color_map, input.color, Output::UNKNOWN); - output.shape = at_or(shape_map, input.shape, Output::UNKNOWN); - output.color = at_or(status_map, input.status, Output::UNKNOWN); + NewElem output; + output.color = at_or(color_map, input.color, NewElem::UNKNOWN); + output.shape = at_or(shape_map, input.shape, NewElem::UNKNOWN); + output.color = at_or(status_map, input.status, NewElem::UNKNOWN); output.confidence = input.confidence; return output; } +NewData convert(const OldData & input) +{ + NewData output; + output.traffic_light_id = input.map_primitive_id; + for (const auto & light : input.lights) { + output.elements.push_back(convert(light)); + } + return output; +} + } // namespace utils TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) @@ -94,10 +106,17 @@ TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) sub_map_ = create_subscription( "~/sub/vector_map", rclcpp::QoS(1).transient_local(), std::bind(&TrafficLightSelector::on_map, this, std::placeholders::_1)); - sub_lights_ = create_subscription( + sub_v2x_ = create_subscription( + "~/sub/traffic_signals", rclcpp::QoS(1), + std::bind(&TrafficLightSelector::on_v2x, this, std::placeholders::_1)); + sub_perception_ = create_subscription( "~/sub/traffic_lights", rclcpp::QoS(1), - std::bind(&TrafficLightSelector::on_lights, this, std::placeholders::_1)); - pub_signals_ = create_publisher("~/pub/traffic_signals", rclcpp::QoS(1)); + std::bind(&TrafficLightSelector::on_perception, this, std::placeholders::_1)); + + pub_ = create_publisher("~/pub/traffic_signals", rclcpp::QoS(1)); + + const auto rate = rclcpp::Rate(1.0); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) @@ -114,8 +133,22 @@ void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) } } -void TrafficLightSelector::on_lights(const TrafficLightArray::ConstSharedPtr msg) +void TrafficLightSelector::on_v2x(const TrafficSignalArray::ConstSharedPtr msg) { data_v2x_ = msg; } + +void TrafficLightSelector::on_perception(const TrafficLightArray2::ConstSharedPtr msg) +{ + const auto data = std::make_shared(); + data->stamp = msg->header.stamp; + data->lights.reserve(msg->signals.size()); + for (const auto & light : msg->signals) { + data->lights.push_back(utils::convert(light)); + } + data_perception_ = data; +} + +void TrafficLightSelector::on_timer() { + /* using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement; @@ -136,6 +169,7 @@ void TrafficLightSelector::on_lights(const TrafficLightArray::ConstSharedPtr msg array.signals.push_back(signal); } pub_signals_->publish(array); + */ } #include diff --git a/perception/traffic_light_selector/src/traffic_light_selector.hpp b/perception/traffic_light_selector/src/traffic_light_selector.hpp index ff4dbbfa8ec43..060bd68f3c667 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.hpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -32,15 +33,23 @@ class TrafficLightSelector : public rclcpp::Node private: using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficLightArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using TrafficLightArray2 = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using TrafficLightArray = autoware_perception_msgs::msg::TrafficLightArray; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_lights_; - rclcpp::Publisher::SharedPtr pub_signals_; + rclcpp::Subscription::SharedPtr sub_v2x_; + rclcpp::Subscription::SharedPtr sub_perception_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; + void on_map(const LaneletMapBin::ConstSharedPtr msg); - void on_lights(const TrafficLightArray::ConstSharedPtr msg); + void on_v2x(const TrafficSignalArray::ConstSharedPtr msg); + void on_perception(const TrafficLightArray2::ConstSharedPtr msg); + void on_timer(); std::unordered_map light_to_signal_; + TrafficSignalArray::ConstSharedPtr data_v2x_; + TrafficLightArray::ConstSharedPtr data_perception_; }; #endif // TRAFFIC_LIGHT_SELECTOR_HPP_ From bed1ed8bcea84582d3b8947a839b8b1de146e8ca Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 Feb 2023 16:29:55 +0900 Subject: [PATCH 04/10] feat: merge traffic signals Signed-off-by: Takagi, Isamu --- .../launch/traffic_light_selector.launch.xml | 1 + .../src/traffic_light_selector.cpp | 30 ++++++++++++------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml index 86817161bcda1..0d4c79bfe90fd 100644 --- a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -3,6 +3,7 @@ + diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index ec07cc597ccc8..28ea3a8911709 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -121,6 +121,7 @@ TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) { + // TODO(Takagi, Isamu): Remove conversion when perception uses new message type. const auto map = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, map); @@ -148,28 +149,37 @@ void TrafficLightSelector::on_perception(const TrafficLightArray2::ConstSharedPt void TrafficLightSelector::on_timer() { - /* - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement; + std::unordered_map> intersections; + + if (data_v2x_) { + for (const auto & signal : data_v2x_->signals) { + auto & elements = intersections[signal.traffic_signal_id]; + for (const auto & element : signal.elements) { + elements.push_back(element); + } + } + } - std::unordered_map> elements; - for (const auto & light : msg->signals) { - const auto id = light_to_signal_[light.map_primitive_id]; - for (const auto & element : light.lights) { - elements[id].push_back(utils::convert(element)); + if (data_perception_) { + for (const auto & light : data_perception_->lights) { + auto & elements = intersections[light_to_signal_[light.traffic_light_id]]; + for (const auto & element : light.elements) { + elements.push_back(element); + } } } + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; TrafficSignalArray array; array.stamp = now(); - for (const auto & [id, elements] : elements) { + for (const auto & [id, elements] : intersections) { TrafficSignal signal; signal.traffic_signal_id = id; signal.elements = elements; array.signals.push_back(signal); } - pub_signals_->publish(array); - */ + pub_->publish(array); } #include From 325a696fd72c4a07e1142d80b409b0ed1ee0acbf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 Feb 2023 16:36:00 +0900 Subject: [PATCH 05/10] feat: ignore when no map Signed-off-by: Takagi, Isamu --- .../traffic_light_selector/src/traffic_light_selector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index 28ea3a8911709..e4509c44df2a3 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -161,7 +161,7 @@ void TrafficLightSelector::on_timer() } } - if (data_perception_) { + if (data_perception_ && !light_to_signal_.empty()) { for (const auto & light : data_perception_->lights) { auto & elements = intersections[light_to_signal_[light.traffic_light_id]]; for (const auto & element : light.elements) { From 45a680c8bb421612a86e3b683e33f09eb87e66b1 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 Feb 2023 16:37:06 +0900 Subject: [PATCH 06/10] feat: modify copyright Signed-off-by: Takagi, Isamu --- .../traffic_light_selector/src/traffic_light_selector.cpp | 2 +- .../traffic_light_selector/src/traffic_light_selector.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index e4509c44df2a3..e1d6849b87729 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/traffic_light_selector/src/traffic_light_selector.hpp b/perception/traffic_light_selector/src/traffic_light_selector.hpp index 060bd68f3c667..f1c0a23ccc6a2 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.hpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From d491f40973cac2ec882d7f83bd78ed89e0a24405 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 9 Mar 2023 15:04:40 +0900 Subject: [PATCH 07/10] feat: add converter Signed-off-by: Takagi, Isamu --- .../traffic_light_selector/CMakeLists.txt | 2 + .../src/traffic_light_converter.cpp | 117 ++++++++++++++++++ .../src/traffic_light_converter.hpp | 36 ++++++ 3 files changed, 155 insertions(+) create mode 100644 perception/traffic_light_selector/src/traffic_light_converter.cpp create mode 100644 perception/traffic_light_selector/src/traffic_light_converter.hpp diff --git a/perception/traffic_light_selector/CMakeLists.txt b/perception/traffic_light_selector/CMakeLists.txt index 39b9d9b4c0f73..9a8555690beed 100644 --- a/perception/traffic_light_selector/CMakeLists.txt +++ b/perception/traffic_light_selector/CMakeLists.txt @@ -5,10 +5,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_converter.cpp src/traffic_light_selector.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} + TrafficLightConverter TrafficLightSelector ) diff --git a/perception/traffic_light_selector/src/traffic_light_converter.cpp b/perception/traffic_light_selector/src/traffic_light_converter.cpp new file mode 100644 index 0000000000000..d67cfaa53de79 --- /dev/null +++ b/perception/traffic_light_selector/src/traffic_light_converter.cpp @@ -0,0 +1,117 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_light_converter.hpp" + +#include +#include + +namespace converter +{ + +using OldList = autoware_auto_perception_msgs::msg::TrafficSignalArray; +using OldData = autoware_auto_perception_msgs::msg::TrafficSignal; +using OldElem = autoware_auto_perception_msgs::msg::TrafficLight; +using NewList = autoware_perception_msgs::msg::TrafficLightArray; +using NewData = autoware_perception_msgs::msg::TrafficLight; +using NewElem = autoware_perception_msgs::msg::TrafficLightElement; + +NewList convert(const OldList & input); +NewData convert(const OldData & input); +NewElem convert(const OldElem & input); + +template +std::vector convert_vector(const L & input) +{ + std::vector output; + output.reserve(input.size()); + for (const auto & value : input) { + output.push_back(convert(value)); + } + return output; +} + +NewList convert(const OldList & input) +{ + NewList output; + output.stamp = input.header.stamp; + output.lights = convert_vector(input.signals); + return output; +} + +NewData convert(const OldData & input) +{ + NewData output; + output.traffic_light_id = input.map_primitive_id; + output.elements = convert_vector(input.lights); + return output; +} + +template +V at_or(const std::unordered_map & map, const K & key, const V & value) +{ + return map.count(key) ? map.at(key) : value; +} + +NewElem convert(const OldElem & input) +{ + // clang-format off + static const std::unordered_map color_map({ + {OldElem::RED, NewElem::RED}, + {OldElem::AMBER, NewElem::AMBER}, + {OldElem::GREEN, NewElem::GREEN}, + {OldElem::WHITE, NewElem::WHITE} + }); + static const std::unordered_map shape_map({ + {OldElem::CIRCLE, NewElem::CIRCLE}, + {OldElem::LEFT_ARROW, NewElem::LEFT_ARROW}, + {OldElem::RIGHT_ARROW, NewElem::RIGHT_ARROW}, + {OldElem::UP_ARROW, NewElem::UP_ARROW}, + {OldElem::DOWN_ARROW, NewElem::DOWN_ARROW}, + {OldElem::DOWN_LEFT_ARROW, NewElem::DOWN_LEFT_ARROW}, + {OldElem::DOWN_RIGHT_ARROW, NewElem::DOWN_RIGHT_ARROW}, + {OldElem::CROSS, NewElem::CROSS} + }); + static const std::unordered_map status_map({ + {OldElem::SOLID_OFF, NewElem::SOLID_OFF}, + {OldElem::SOLID_ON, NewElem::SOLID_ON}, + {OldElem::FLASHING, NewElem::FLASHING} + }); + // clang-format on + + NewElem output; + output.color = at_or(color_map, input.color, NewElem::UNKNOWN); + output.shape = at_or(shape_map, input.shape, NewElem::UNKNOWN); + output.status = at_or(status_map, input.status, NewElem::UNKNOWN); + output.confidence = input.confidence; + return output; +} + +} // namespace converter + +TrafficLightConverter::TrafficLightConverter(const rclcpp::NodeOptions & options) +: Node("traffic_light_converter", options) +{ + const auto callback = std::bind(&TrafficLightConverter::on_msg, this, std::placeholders::_1); + sub_ = create_subscription("~/sub/traffic_lights", rclcpp::QoS(1), callback); + pub_ = create_publisher("~/pub/traffic_lights", rclcpp::QoS(1)); +} + +void TrafficLightConverter::on_msg(const OldMessage::ConstSharedPtr msg) +{ + pub_->publish(converter::convert(*msg)); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightConverter) diff --git a/perception/traffic_light_selector/src/traffic_light_converter.hpp b/perception/traffic_light_selector/src/traffic_light_converter.hpp new file mode 100644 index 0000000000000..db6d0a76c9c52 --- /dev/null +++ b/perception/traffic_light_selector/src/traffic_light_converter.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_LIGHT_CONVERTER_HPP_ +#define TRAFFIC_LIGHT_CONVERTER_HPP_ + +#include + +#include +#include + +class TrafficLightConverter : public rclcpp::Node +{ +public: + explicit TrafficLightConverter(const rclcpp::NodeOptions & options); + +private: + using OldMessage = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using NewMessage = autoware_perception_msgs::msg::TrafficLightArray; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + void on_msg(const OldMessage::ConstSharedPtr msg); +}; + +#endif // TRAFFIC_LIGHT_CONVERTER_HPP_ From 3999a2092e99e8a665e1c294ba8e04742e6c8d0b Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 9 Mar 2023 20:21:17 +0900 Subject: [PATCH 08/10] feat: add selector Signed-off-by: Takagi, Isamu --- .../launch/traffic_light_selector.launch.xml | 10 +- .../src/traffic_light_selector.cpp | 147 ++++++------------ .../src/traffic_light_selector.hpp | 14 +- 3 files changed, 55 insertions(+), 116 deletions(-) diff --git a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml index 0d4c79bfe90fd..44eec5284c03c 100644 --- a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -1,10 +1,14 @@ - - - + + + + + + + diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp index e1d6849b87729..599bcc0f389f1 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.cpp @@ -18,7 +18,9 @@ #include +#include #include +#include #include namespace lanelet @@ -40,143 +42,84 @@ std::vector filter_traffic_signals(const LaneletMapConstPt } // namespace lanelet -namespace utils -{ - -using OldData = autoware_auto_perception_msgs::msg::TrafficSignal; -using OldElem = autoware_auto_perception_msgs::msg::TrafficLight; -using NewData = autoware_perception_msgs::msg::TrafficLight; -using NewElem = autoware_perception_msgs::msg::TrafficLightElement; - -template -V at_or(const std::unordered_map & map, const K & key, const V & value) -{ - return map.count(key) ? map.at(key) : value; -} - -NewElem convert(const OldElem & input) -{ - // clang-format off - static const std::unordered_map color_map({ - {OldElem::RED, NewElem::RED}, - {OldElem::AMBER, NewElem::AMBER}, - {OldElem::GREEN, NewElem::GREEN}, - {OldElem::WHITE, NewElem::WHITE} - }); - static const std::unordered_map shape_map({ - {OldElem::CIRCLE, NewElem::CIRCLE}, - {OldElem::LEFT_ARROW, NewElem::LEFT_ARROW}, - {OldElem::RIGHT_ARROW, NewElem::RIGHT_ARROW}, - {OldElem::UP_ARROW, NewElem::UP_ARROW}, - {OldElem::DOWN_ARROW, NewElem::DOWN_ARROW}, - {OldElem::DOWN_LEFT_ARROW, NewElem::DOWN_LEFT_ARROW}, - {OldElem::DOWN_RIGHT_ARROW, NewElem::DOWN_RIGHT_ARROW}, - {OldElem::CROSS, NewElem::CROSS} - }); - static const std::unordered_map status_map({ - {OldElem::SOLID_OFF, NewElem::SOLID_OFF}, - {OldElem::SOLID_ON, NewElem::SOLID_ON}, - {OldElem::FLASHING, NewElem::FLASHING} - }); - // clang-format on - - NewElem output; - output.color = at_or(color_map, input.color, NewElem::UNKNOWN); - output.shape = at_or(shape_map, input.shape, NewElem::UNKNOWN); - output.color = at_or(status_map, input.status, NewElem::UNKNOWN); - output.confidence = input.confidence; - return output; -} - -NewData convert(const OldData & input) -{ - NewData output; - output.traffic_light_id = input.map_primitive_id; - for (const auto & light : input.lights) { - output.elements.push_back(convert(light)); - } - return output; -} - -} // namespace utils - TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) : Node("traffic_light_selector", options) { sub_map_ = create_subscription( "~/sub/vector_map", rclcpp::QoS(1).transient_local(), std::bind(&TrafficLightSelector::on_map, this, std::placeholders::_1)); - sub_v2x_ = create_subscription( - "~/sub/traffic_signals", rclcpp::QoS(1), - std::bind(&TrafficLightSelector::on_v2x, this, std::placeholders::_1)); - sub_perception_ = create_subscription( + + sub_tlr_ = create_subscription( "~/sub/traffic_lights", rclcpp::QoS(1), - std::bind(&TrafficLightSelector::on_perception, this, std::placeholders::_1)); + std::bind(&TrafficLightSelector::on_msg, this, std::placeholders::_1)); pub_ = create_publisher("~/pub/traffic_signals", rclcpp::QoS(1)); - - const auto rate = rclcpp::Rate(1.0); - timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) { - // TODO(Takagi, Isamu): Remove conversion when perception uses new message type. const auto map = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - light_to_signal_.clear(); + mapping_.clear(); for (const auto & signal : signals) { for (const auto & light : signal->trafficLights()) { - light_to_signal_[light.id()] = signal->id(); + mapping_[light.id()] = signal->id(); } } } -void TrafficLightSelector::on_v2x(const TrafficSignalArray::ConstSharedPtr msg) { data_v2x_ = msg; } - -void TrafficLightSelector::on_perception(const TrafficLightArray2::ConstSharedPtr msg) -{ - const auto data = std::make_shared(); - data->stamp = msg->header.stamp; - data->lights.reserve(msg->signals.size()); - for (const auto & light : msg->signals) { - data->lights.push_back(utils::convert(light)); - } - data_perception_ = data; -} - -void TrafficLightSelector::on_timer() +void TrafficLightSelector::on_msg(const TrafficLightArray::ConstSharedPtr msg) { - using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement; - std::unordered_map> intersections; - - if (data_v2x_) { - for (const auto & signal : data_v2x_->signals) { - auto & elements = intersections[signal.traffic_signal_id]; - for (const auto & element : signal.elements) { - elements.push_back(element); + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using Element = autoware_perception_msgs::msg::TrafficLightElement; + std::unordered_map> intersections; + + // Use the most confident traffic light element in the same state. + const auto get_highest_confidence_elements = [](const std::vector & elements) { + using Key = std::tuple; + std::map highest_; + + for (const auto & element : elements) { + const auto key = std::make_tuple(element.color, element.shape, element.status); + auto [iter, success] = highest_.try_emplace(key, element); + if (!success && iter->second.confidence < element.confidence) { + iter->second = element; } } + + std::vector result; + result.reserve(highest_.size()); + for (const auto & [k, v] : highest_) { + result.push_back(v); + } + return result; + }; + + // Wait for vector map to create id mapping. + if (mapping_.empty()) { + return; } - if (data_perception_ && !light_to_signal_.empty()) { - for (const auto & light : data_perception_->lights) { - auto & elements = intersections[light_to_signal_[light.traffic_light_id]]; - for (const auto & element : light.elements) { - elements.push_back(element); - } + // Merge traffic lights in the same group. + for (const auto & light : msg->lights) { + const auto id = light.traffic_light_id; + if (!mapping_.count(id)) { + continue; + } + auto & elements = intersections[mapping_[id]]; + for (const auto & element : light.elements) { + elements.push_back(element); } } - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; TrafficSignalArray array; - array.stamp = now(); + array.stamp = msg->stamp; for (const auto & [id, elements] : intersections) { TrafficSignal signal; signal.traffic_signal_id = id; - signal.elements = elements; + signal.elements = get_highest_confidence_elements(elements); array.signals.push_back(signal); } pub_->publish(array); diff --git a/perception/traffic_light_selector/src/traffic_light_selector.hpp b/perception/traffic_light_selector/src/traffic_light_selector.hpp index f1c0a23ccc6a2..b21efaa2ec2e3 100644 --- a/perception/traffic_light_selector/src/traffic_light_selector.hpp +++ b/perception/traffic_light_selector/src/traffic_light_selector.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include @@ -33,23 +32,16 @@ class TrafficLightSelector : public rclcpp::Node private: using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficLightArray2 = autoware_auto_perception_msgs::msg::TrafficSignalArray; using TrafficLightArray = autoware_perception_msgs::msg::TrafficLightArray; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_v2x_; - rclcpp::Subscription::SharedPtr sub_perception_; + rclcpp::Subscription::SharedPtr sub_tlr_; rclcpp::Publisher::SharedPtr pub_; - rclcpp::TimerBase::SharedPtr timer_; void on_map(const LaneletMapBin::ConstSharedPtr msg); - void on_v2x(const TrafficSignalArray::ConstSharedPtr msg); - void on_perception(const TrafficLightArray2::ConstSharedPtr msg); - void on_timer(); + void on_msg(const TrafficLightArray::ConstSharedPtr msg); - std::unordered_map light_to_signal_; - TrafficSignalArray::ConstSharedPtr data_v2x_; - TrafficLightArray::ConstSharedPtr data_perception_; + std::unordered_map mapping_; }; #endif // TRAFFIC_LIGHT_SELECTOR_HPP_ From 2f862d2fd36fda2ad0ea63f06166fe4f63fdf768 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 9 Mar 2023 20:41:36 +0900 Subject: [PATCH 09/10] feat: add readme Signed-off-by: Takagi, Isamu --- perception/traffic_light_selector/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 perception/traffic_light_selector/README.md diff --git a/perception/traffic_light_selector/README.md b/perception/traffic_light_selector/README.md new file mode 100644 index 0000000000000..19d4b180f7ed5 --- /dev/null +++ b/perception/traffic_light_selector/README.md @@ -0,0 +1,9 @@ +# traffic_light_selector + +## selector node + +A node that merges traffic light data from traffic light recognition and V2X to provide a planning component. + +## converter node + +A temporary node that converts old message (autoware_auto_perception_msgs) to new message (autoware_perception_msgs). From b95cc297bd77fc3d159e67ed801dbcdf7c0a03be Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 10 Mar 2023 17:35:16 +0900 Subject: [PATCH 10/10] docs: update readme Signed-off-by: Takagi, Isamu --- perception/traffic_light_selector/README.md | 42 +++++++++++++++++++-- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/perception/traffic_light_selector/README.md b/perception/traffic_light_selector/README.md index 19d4b180f7ed5..9e38333c0e345 100644 --- a/perception/traffic_light_selector/README.md +++ b/perception/traffic_light_selector/README.md @@ -1,9 +1,43 @@ # traffic_light_selector -## selector node +## Purpose -A node that merges traffic light data from traffic light recognition and V2X to provide a planning component. +This package receives multiple traffic light/signal states and outputs a single traffic signal state for use by the planning component. -## converter node +## TrafficLightSelector -A temporary node that converts old message (autoware_auto_perception_msgs) to new message (autoware_perception_msgs). +A node that merges traffic light/signal state from image recognition and V2X to provide a planning component. +It's currently a provisional implementation. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| -------------------- | ---------------------------------------------- | ------------------------------------------------- | +| ~/sub/vector_map | autoware_auto_mapping_msgs/msg/HADMapBin | The vector map to get traffic light id relations. | +| ~/sub/traffic_lights | autoware_perception_msgs/msg/TrafficLightArray | The traffic light state from image recognition. | + +#### Output + +| Name | Type | Description | +| --------------------- | ----------------------------------------------- | -------------------------------- | +| ~/pub/traffic_signals | autoware_perception_msgs/msg/TrafficSignalArray | The merged traffic signal state. | + +## TrafficLightConverter + +A temporary node that converts old message type to new message type. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| -------------------- | ---------------------------------------------------- | ------------------------------ | +| ~/sub/traffic_lights | autoware_auto_perception_msgs/msg/TrafficSignalArray | The state in old message type. | + +#### Output + +| Name | Type | Description | +| -------------------- | ---------------------------------------------- | ------------------------------ | +| ~/pub/traffic_lights | autoware_perception_msgs/msg/TrafficLightArray | The state in new message type. |