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

autowarefoundation#9721) * feat: add traffic light selector node Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> feat: add traffic ligth selector node Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add check expect roi iou Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: tl selector Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: launch file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: update matching score Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: calc sum IOU for whole shifted image Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: check inside rough roi Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: check inside function Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * feat: add max_iou_threshold Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: pre-commit Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: add readme Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: launch file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: pre-commit Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add unknown in selector Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: change to GenIOU Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * feat: add debug topic Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add maintainer Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: pre-commit Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix:cmake Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: move param to yaml file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add schema Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp> * style(pre-commit): autofix * fix typo Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp> Co-authored-by: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Co-authored-by: MasatoSaeki <masato.saeki@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Feb 10, 2025 · b59cd8d · b59cd8d
1 parent 7f9cdb6
commit b59cd8d
Show file tree

Hide file tree

Showing 10 changed files with 626 additions and 0 deletions.
diff --git a/perception/autoware_traffic_light_selector/CMakeLists.txt b/perception/autoware_traffic_light_selector/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_traffic_light_selector)
+
+# find dependencies
+find_package(autoware_cmake REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Sophus REQUIRED)
+find_package(Boost REQUIRED)
+find_package(PCL REQUIRED)
+find_package(CGAL REQUIRED COMPONENTS Core)
+
+include_directories(
+  include
+  SYSTEM
+  ${Boost_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+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
+  config
+)
diff --git a/perception/autoware_traffic_light_selector/README.md b/perception/autoware_traffic_light_selector/README.md
@@ -0,0 +1,23 @@
+`autoware_traffic_light_selector`
+
+## Overview
+
+`autoware_traffic_light_selector` selects the interest traffic light from the list of accurately detected traffic lights by something (e.g. deep learning neural network) 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 | accurately detected traffic light                                    |
+| `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_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest with id |
+
+## Node parameters
+
+{{json_to_markdown("perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json")}}
diff --git a/perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml b/perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    max_iou_threshold: -0.5
diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml
@@ -0,0 +1,15 @@
+<launch>
+  <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"/>
+  <arg name="output/traffic_rois" default="output/traffic_light_rois"/>
+  <arg name="traffic_light_selector_param_path" default="$(find-pkg-share autoware_traffic_light_selector)/config/traffic_light_selector.param.yaml"/>
+
+  <node pkg="autoware_traffic_light_selector" exec="traffic_light_selector_node" name="traffic_light_selector" output="screen">
+    <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"/>
+    <remap from="output/traffic_rois" to="$(var output/traffic_rois)"/>
+    <param from="$(var traffic_light_selector_param_path)"/>
+  </node>
+</launch>
diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml
@@ -0,0 +1,33 @@
+<?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 autoware_traffic_light_selector package</description>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="dai.nguyen@tier4.jp">Dai Nguyen</maintainer>
+  <maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
+  <maintainer email="masato.saeki@tier4.jp">Masato Saeki</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="dai.nguyen@tier4.jp">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>
diff --git a/perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json b/perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json
@@ -0,0 +1,32 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for traffic_light_selector",
+  "type": "object",
+  "definitions": {
+    "traffic_light_selector": {
+      "type": "object",
+      "properties": {
+        "max_iou_threshold": {
+          "type": "number",
+          "description": "Threshold of generalized IOU for detected traffic light selection",
+          "default": -0.5,
+          "minimum": -1.0,
+          "maximum": 1.0
+        }
+      },
+      "required": ["max_iou_threshold"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/traffic_light_selector"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}
diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp
@@ -0,0 +1,182 @@
+// 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 <memory>
+#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_)
+{
+  {
+    stop_watch_ptr_ =
+      std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ =
+      std::make_unique<autoware::universe_utils::DebugPublisher>(this, this->get_name());
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
+  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_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)
+{
+  stop_watch_ptr_->toc("processing_time", true);
+  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);
+  if (debug_publisher_) {
+    const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+    const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+    const double pipeline_latency_ms =
+      std::chrono::duration<double, std::milli>(
+        std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds()))
+        .count();
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/pipeline_latency_ms", pipeline_latency_ms);
+  }
+  return;
+}
+}  // namespace autoware::traffic_light
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightSelectorNode)