feat: add traffic signals merge node

perception/traffic_light_selector/CMakeLists.txt

@@ -0,0 +1,17 @@
+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_converter.cpp
+  src/traffic_light_selector.cpp
+)
+
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  TrafficLightConverter
+  TrafficLightSelector
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)

perception/traffic_light_selector/README.md

@@ -0,0 +1,43 @@
+# traffic_light_selector
+
+## Purpose
+
+This package receives multiple traffic light/signal states and outputs a single traffic signal state for use by the planning component.
+
+## TrafficLightSelector
+
+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. |

perception/traffic_light_selector/launch/traffic_light_selector.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <push-ros-namespace namespace="traffic_light_selector"/>
+  <node_container pkg="rclcpp_components" exec="component_container" name="container" namespace="">
+    <composable_node pkg="traffic_light_selector" plugin="TrafficLightConverter" name="converter">
+      <remap from="~/sub/traffic_lights" to="/perception/traffic_light_recognition/traffic_signals"/>
+      <remap from="~/pub/traffic_lights" to="/perception/traffic_light_selector/internal/traffic_lights"/>
+    </composable_node>
+    <composable_node pkg="traffic_light_selector" plugin="TrafficLightSelector" name="selector">
+      <remap from="~/sub/vector_map" to="/map/vector_map"/>
+      <remap from="~/sub/traffic_lights" to="/perception/traffic_light_selector/internal/traffic_lights"/>
+      <remap from="~/pub/traffic_signals" to="/perception/traffic_light_selector/traffic_signals"/>
+    </composable_node>
+  </node_container>
+</launch>

perception/traffic_light_selector/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>traffic_light_selector</name>
+  <version>0.1.0</version>
+  <description>The traffic_light_selector package</description>
+  <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>lanelet2_core</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <unordered_map>
+#include <vector>
+
+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 <class T, class L>
+std::vector<T> convert_vector(const L & input)
+{
+  std::vector<T> 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<NewData>(input.signals);
+  return output;
+}
+
+NewData convert(const OldData & input)
+{
+  NewData output;
+  output.traffic_light_id = input.map_primitive_id;
+  output.elements = convert_vector<NewElem>(input.lights);
+  return output;
+}
+
+template <class K, class V>
+V at_or(const std::unordered_map<K, V> & 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<OldElem::_color_type, NewElem::_color_type> color_map({
+    {OldElem::RED, NewElem::RED},
+    {OldElem::AMBER, NewElem::AMBER},
+    {OldElem::GREEN, NewElem::GREEN},
+    {OldElem::WHITE, NewElem::WHITE}
+  });
+  static const std::unordered_map<OldElem::_shape_type, NewElem::_shape_type> 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<OldElem::_status_type, NewElem::_status_type> 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<OldMessage>("~/sub/traffic_lights", rclcpp::QoS(1), callback);
+  pub_ = create_publisher<NewMessage>("~/pub/traffic_lights", rclcpp::QoS(1));
+}
+
+void TrafficLightConverter::on_msg(const OldMessage::ConstSharedPtr msg)
+{
+  pub_->publish(converter::convert(*msg));
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightConverter)

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_array.hpp>
+
+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<OldMessage>::SharedPtr sub_;
+  rclcpp::Publisher<NewMessage>::SharedPtr pub_;
+  void on_msg(const OldMessage::ConstSharedPtr msg);
+};
+
+#endif  // TRAFFIC_LIGHT_CONVERTER_HPP_

perception/traffic_light_selector/src/traffic_light_selector.cpp

@@ -0,0 +1,129 @@
+// 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_selector.hpp"
+
+#include <lanelet2_extension/utility/message_conversion.hpp>
+
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+
+#include <map>
+#include <memory>
+#include <tuple>
+#include <vector>
+
+namespace lanelet
+{
+
+using TrafficLightConstPtr = std::shared_ptr<const TrafficLight>;
+
+std::vector<TrafficLightConstPtr> filter_traffic_signals(const LaneletMapConstPtr map)
+{
+  std::vector<TrafficLightConstPtr> signals;
+  for (const auto & element : map->regulatoryElementLayer) {
+    const auto signal = std::dynamic_pointer_cast<const TrafficLight>(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<LaneletMapBin>(
+    "~/sub/vector_map", rclcpp::QoS(1).transient_local(),
+    std::bind(&TrafficLightSelector::on_map, this, std::placeholders::_1));
+
+  sub_tlr_ = create_subscription<TrafficLightArray>(
+    "~/sub/traffic_lights", rclcpp::QoS(1),
+    std::bind(&TrafficLightSelector::on_msg, this, std::placeholders::_1));
+
+  pub_ = create_publisher<TrafficSignalArray>("~/pub/traffic_signals", rclcpp::QoS(1));
+}
+
+void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg)
+{
+  const auto map = std::make_shared<lanelet::LaneletMap>();
+  lanelet::utils::conversion::fromBinMsg(*msg, map);
+
+  const auto signals = lanelet::filter_traffic_signals(map);
+  mapping_.clear();
+  for (const auto & signal : signals) {
+    for (const auto & light : signal->trafficLights()) {
+      mapping_[light.id()] = signal->id();
+    }
+  }
+}
+
+void TrafficLightSelector::on_msg(const TrafficLightArray::ConstSharedPtr msg)
+{
+  using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
+  using Element = autoware_perception_msgs::msg::TrafficLightElement;
+  std::unordered_map<lanelet::Id, std::vector<Element>> intersections;
+
+  // Use the most confident traffic light element in the same state.
+  const auto get_highest_confidence_elements = [](const std::vector<Element> & elements) {
+    using Key = std::tuple<Element::_color_type, Element::_shape_type, Element::_status_type>;
+    std::map<Key, Element> 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<Element> 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;
+  }
+
+  // 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);
+    }
+  }
+
+  TrafficSignalArray array;
+  array.stamp = msg->stamp;
+  for (const auto & [id, elements] : intersections) {
+    TrafficSignal signal;
+    signal.traffic_signal_id = id;
+    signal.elements = get_highest_confidence_elements(elements);
+    array.signals.push_back(signal);
+  }
+  pub_->publish(array);
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightSelector)