feat(traffic_light_selector): add new node for traffic light selection

perception/autoware_traffic_light_selector/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_traffic_light_selector)
+
+# find dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/traffic_light_selector_node.cpp
+  src/utils.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::traffic_light::TrafficLightSelectorNode"
+  EXECUTABLE traffic_light_selector_node)
+
+
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

perception/autoware_traffic_light_selector/README.md

@@ -0,0 +1,23 @@
+# The `autoware_traffic_light_selector` Package
+
+## Overview
+
+`autoware_traffic_light_selector` selects the interest traffic light from the list of detected traffic lights by deep learning neural network (DNN) based on the expect ROIs and rough ROIs information and then assign traffic_light_id for them.
+
+## Input topics
+
+| Name                  | Type                                                   | Description                                                          |
+| --------------------- | ------------------------------------------------------ | -------------------------------------------------------------------- |
+| `input/detected_rois` | tier4_perception_msgs::msg::DetectedObjectsWithFeature | detected traffic light by DNN                                        |
+| `input/rough_rois`    | tier4_perception_msgs::msg::TrafficLightRoiArray       | location of traffic lights in image corresponding to the camera info |
+| `input/expect_rois`   | tier4_perception_msgs::msg::TrafficLightRoiArray       | location of traffic lights in image without any offset               |
+
+## Output topics
+
+| Name                        | Type                                        | Description                        |
+| --------------------------- | ------------------------------------------- | ---------------------------------- |
+| `output/traffic_light_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest |
+
+## Node parameters
+
+N/A

perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <arg name="output/traffic_light_rois" default="output/traffic_light_rois"/>
+  <arg name="input/detected_rois" default="input/detected_rois"/>
+  <arg name="input/rough_rois" default="input/rough_rois"/>
+  <arg name="input/camera_info" default="input/camera_info"/>
+
+  <node pkg="autoware_traffic_light_selector" exec="traffic_light_selector_node" name="autoware_traffic_light_selector" output="screen">
+    <remap from="output/traffic_light_rois" to="$(var output/traffic_light_rois)"/>
+    <remap from="input/detected_rois" to="$(var input/detected_rois)"/>
+    <remap from="input/rough_rois" to="$(var input/rough_rois)"/>
+    <remap from="input/camera_info" to="input/camera_info"/>
+    <param name="max_iou_threshold" value="-0.5"/>
+  </node>
+</launch>

perception/autoware_traffic_light_selector/package.xml

@@ -0,0 +1,32 @@
+<?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>autoware_traffic_light_selector</name>
+  <version>0.1.0</version>
+  <description>The ROS 2 cluster merger package</description>
+  <maintainer email="[email protected]">Yukihiro Saito</maintainer>
+  <maintainer email="[email protected]">Dai Nguyen</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Dai Nguyen</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_test_utils</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>cv_bridge</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libopencv-dev</depend>
+  <depend>message_filters</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tier4_perception_msgs</depend>
+  <build_depend>autoware_cmake</build_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/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp

@@ -0,0 +1,157 @@
+// Copyright 2025 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_node.hpp"
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <sensor_msgs/msg/region_of_interest.hpp>
+
+#include <map>
+#include <vector>
+
+namespace autoware::traffic_light
+{
+
+TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & node_options)
+: rclcpp::Node("traffic_light_selector_node", node_options),
+  tf_buffer_(get_clock()),
+  tf_listener_(tf_buffer_),
+  detected_rois_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  rough_rois_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_)
+{
+  max_iou_threshold_ = declare_parameter<double>("max_iou_threshold");
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  using std::placeholders::_3;
+  sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3));
+
+  camera_info_sub_ = create_subscription<sensor_msgs::msg::CameraInfo>(
+    "input/camera_info", rclcpp::SensorDataQoS(),
+    std::bind(&TrafficLightSelectorNode::cameraInfoCallback, this, _1));
+  // Publisher
+  pub_traffic_light_rois_ =
+    create_publisher<TrafficLightRoiArray>("output/traffic_light_rois", rclcpp::QoS{1});
+}
+
+void TrafficLightSelectorNode::cameraInfoCallback(
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg)
+{
+  if (camera_info_subscribed_) {
+    return;
+  }
+  RCLCPP_INFO(get_logger(), "camera_info received");
+  image_width_ = input_msg->width;
+  image_height_ = input_msg->height;
+  camera_info_subscribed_ = true;
+}
+
+void TrafficLightSelectorNode::objectsCallback(
+  const DetectedObjectsWithFeature::ConstSharedPtr & detected_traffic_light_msg,
+  const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg,
+  const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg)
+{
+  if (!camera_info_subscribed_) {
+    return;
+  }
+  std::map<int, sensor_msgs::msg::RegionOfInterest> rough_rois_map;
+  for (const auto & roi : rough_rois_msg->rois) {
+    rough_rois_map[roi.traffic_light_id] = roi.roi;
+  }
+  // TODO(badai-nguyen): implement this function on CUDA or refactor the code
+
+  TrafficLightRoiArray output;
+  output.header = detected_traffic_light_msg->header;
+  float max_matching_score = 0.0;
+  int det_roi_shift_x = 0;
+  int det_roi_shift_y = 0;
+  std::vector<sensor_msgs::msg::RegionOfInterest> det_rois;
+  std::vector<sensor_msgs::msg::RegionOfInterest> expect_rois;
+  for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) {
+    det_rois.push_back(detected_roi.feature.roi);
+  }
+  for (const auto & expected_roi : expected_rois_msg->rois) {
+    expect_rois.push_back(expected_roi.roi);
+  }
+  cv::Mat expect_roi_img =
+    utils::createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_));
+  cv::Mat det_roi_img =
+    utils::createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_));
+  // for (const auto expect_roi : expect_rois) {
+  for (const auto & expect_traffic_roi : expected_rois_msg->rois) {
+    const auto & expect_roi = expect_traffic_roi.roi;
+    auto traffic_light_id = expect_traffic_roi.traffic_light_id;
+    const auto & rough_roi = rough_rois_map[traffic_light_id];
+
+    for (const auto & detected_roi : det_rois) {
+      // check if the detected roi is inside the rough roi
+      if (!utils::isInsideRoughRoi(detected_roi, rough_roi)) {
+        continue;
+      }
+      int dx = static_cast<int>(detected_roi.x_offset) - static_cast<int>(expect_roi.x_offset);
+      int dy = static_cast<int>(detected_roi.y_offset) - static_cast<int>(expect_roi.y_offset);
+      cv::Mat det_roi_shifted = utils::shiftAndPaddingImage(det_roi_img, dx, dy);
+      double iou = utils::getIoUOf2BinaryImages(expect_roi_img, det_roi_shifted);
+      if (iou > max_matching_score) {
+        max_matching_score = iou;
+        det_roi_shift_x = dx;
+        det_roi_shift_y = dy;
+      }
+    }
+  }
+
+  for (const auto & expect_roi : expected_rois_msg->rois) {
+    // check max IOU after shift
+    double max_iou = -1.0;
+    sensor_msgs::msg::RegionOfInterest max_iou_roi;
+    for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) {
+      // shift detected roi by det_roi_shift_x, det_roi_shift_y and calculate IOU
+      sensor_msgs::msg::RegionOfInterest detected_roi_shifted = detected_roi.feature.roi;
+      // fit top lef corner of detected roi to inside of image
+      detected_roi_shifted.x_offset = std::clamp(
+        static_cast<int>(detected_roi.feature.roi.x_offset) - det_roi_shift_x, 0,
+        static_cast<int>(image_width_ - detected_roi.feature.roi.width));
+      detected_roi_shifted.y_offset = std::clamp(
+        static_cast<int>(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0,
+        static_cast<int>(image_height_ - detected_roi.feature.roi.height));
+
+      double iou = utils::getGenIoU(expect_roi.roi, detected_roi_shifted);
+      if (iou > max_iou) {
+        max_iou = iou;
+        max_iou_roi = detected_roi.feature.roi;
+      }
+    }
+    if (max_iou > max_iou_threshold_) {
+      TrafficLightRoi traffic_light_roi;
+      traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id;
+      traffic_light_roi.traffic_light_type = expect_roi.traffic_light_type;
+      traffic_light_roi.roi = max_iou_roi;
+      output.rois.push_back(traffic_light_roi);
+    } else {
+      TrafficLightRoi traffic_light_roi;
+      traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id;
+      traffic_light_roi.traffic_light_type = expect_roi.traffic_light_type;
+      output.rois.push_back(traffic_light_roi);
+    }
+  }
+  pub_traffic_light_rois_->publish(output);
+  return;
+}
+}  // namespace autoware::traffic_light
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightSelectorNode)

perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp

@@ -0,0 +1,79 @@
+// Copyright 2025 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_NODE_HPP_
+#define TRAFFIC_LIGHT_SELECTOR_NODE_HPP_
+
+#include "utils.hpp"
+
+#include <autoware/universe_utils/ros/transform_listener.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <tier4_perception_msgs/msg/detected_object_with_feature.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <tier4_perception_msgs/msg/traffic_light_roi.hpp>
+#include <tier4_perception_msgs/msg/traffic_light_roi_array.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace autoware::traffic_light
+{
+using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
+using tier4_perception_msgs::msg::DetectedObjectWithFeature;
+using tier4_perception_msgs::msg::TrafficLightRoi;
+using tier4_perception_msgs::msg::TrafficLightRoiArray;
+
+class TrafficLightSelectorNode : public rclcpp::Node
+{
+public:
+  explicit TrafficLightSelectorNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Subscriber
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  message_filters::Subscriber<DetectedObjectsWithFeature> detected_rois_sub_;
+  message_filters::Subscriber<TrafficLightRoiArray> rough_rois_sub_;
+  message_filters::Subscriber<TrafficLightRoiArray> expected_rois_sub_;
+  typedef message_filters::sync_policies::ApproximateTime<
+    DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray>
+    SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Sync;
+  Sync sync_;
+  void objectsCallback(
+    const DetectedObjectsWithFeature::ConstSharedPtr & detected_rois_msg,
+    const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg,
+    const TrafficLightRoiArray::ConstSharedPtr & expect_rois_msg);
+
+  void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg);
+  // Publisher
+  rclcpp::Publisher<TrafficLightRoiArray>::SharedPtr pub_traffic_light_rois_;
+  // Subscribe camera_info to get width and height of image
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
+  bool camera_info_subscribed_;
+  uint32_t image_width_{1280};
+  uint32_t image_height_{960};
+  double max_iou_threshold_{0.0};
+};
+
+}  // namespace autoware::traffic_light
+
+#endif  // TRAFFIC_LIGHT_SELECTOR_NODE_HPP_