diff --git a/perception/autoware_traffic_light_category_merger/CMakeLists.txt b/perception/autoware_traffic_light_category_merger/CMakeLists.txt new file mode 100644 index 0000000000000..af17d1f4705a1 --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_traffic_light_category_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_category_merger_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightCategoryMergerNode" + EXECUTABLE traffic_light_category_merger_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 +) diff --git a/perception/autoware_traffic_light_category_merger/README.md b/perception/autoware_traffic_light_category_merger/README.md new file mode 100644 index 0000000000000..260ee868ad067 --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/README.md @@ -0,0 +1,22 @@ +`autoware_traffic_light_category_merger` + +## Overview + +`autoware_traffic_light_category_merger` receives the Traffic Light (TL) classification result from Car/Pedestrian classifiers to merge into single classification result. The expect ROIs TL without classification result will be filled as Unknown. + +## Input topics + +| Name | Type | Description | +| -------------------------- | --------------------------------------------- | ----------------------------- | +| `input/car_signals` | tier4_perception_msgs::msg::TrafficLightArray | Car TLs classification | +| `input/pedestrian_signals` | tier4_perception_msgs::msg::TrafficLightArray | Pedestrian TLs classification | + +## Output topics + +| Name | Type | Description | +| ------------------------ | --------------------------------------------- | ------------------------------------- | +| `output/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | Car and Pedestrian TLs classification | + +## Node parameters + +N/A diff --git a/perception/autoware_traffic_light_category_merger/launch/traffic_light_category_merger.launch.xml b/perception/autoware_traffic_light_category_merger/launch/traffic_light_category_merger.launch.xml new file mode 100644 index 0000000000000..0ab1fbdf6cb96 --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/launch/traffic_light_category_merger.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/perception/autoware_traffic_light_category_merger/package.xml b/perception/autoware_traffic_light_category_merger/package.xml new file mode 100644 index 0000000000000..53f04c05dc42c --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/package.xml @@ -0,0 +1,25 @@ + + + + autoware_traffic_light_category_merger + 0.0.0 + The ROS 2 autoware_traffic_light_category_merger package + Yoshi Ri + Masato Saeki + Dai Nguyen + Apache License 2.0 + Dai Nguyen + + ament_cmake_auto + + autoware_test_utils + message_filters + rclcpp + rclcpp_components + tier4_perception_msgs + autoware_cmake + + + ament_cmake + + diff --git a/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.cpp b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.cpp new file mode 100644 index 0000000000000..114542ae55a42 --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.cpp @@ -0,0 +1,57 @@ +// 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_category_merger_node.hpp" + +#include +#include +#include + +namespace autoware::traffic_light +{ + +TrafficLightCategoryMergerNode::TrafficLightCategoryMergerNode( + const rclcpp::NodeOptions & node_options) +: rclcpp::Node("traffic_light_category_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + car_signal_sub_(this, "input/car_signals", rclcpp::QoS{1}.get_rmw_qos_profile()), + pedestrian_signal_sub_(this, "input/pedestrian_signals", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), car_signal_sub_, pedestrian_signal_sub_) +{ + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&TrafficLightCategoryMergerNode::signalsCallback, this, _1, _2)); + pub_traffic_light_signals_ = + create_publisher("output/traffic_signals", rclcpp::QoS{1}); +} + +void TrafficLightCategoryMergerNode::signalsCallback( + const TrafficLightArray::ConstSharedPtr & car_signals_msg, + const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg) +{ + TrafficLightArray output; + output.header = car_signals_msg->header; + output.signals.insert( + output.signals.end(), car_signals_msg->signals.begin(), car_signals_msg->signals.end()); + output.signals.insert( + output.signals.end(), pedestrian_signals_msg->signals.begin(), + pedestrian_signals_msg->signals.end()); + pub_traffic_light_signals_->publish(output); +} + +} // namespace autoware::traffic_light + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightCategoryMergerNode) diff --git a/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp new file mode 100644 index 0000000000000..23aa62fdcd2a0 --- /dev/null +++ b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp @@ -0,0 +1,62 @@ +// 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_CATEGORY_MERGER_NODE_HPP_ +#define TRAFFIC_LIGHT_CATEGORY_MERGER_NODE_HPP_ + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::traffic_light +{ +using tier4_perception_msgs::msg::TrafficLightArray; + +class TrafficLightCategoryMergerNode : public rclcpp::Node +{ +public: + explicit TrafficLightCategoryMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + message_filters::Subscriber car_signal_sub_; + message_filters::Subscriber pedestrian_signal_sub_; + + typedef message_filters::sync_policies::ApproximateTime + SyncPolicy; + message_filters::Synchronizer sync_; + + void signalsCallback( + const TrafficLightArray::ConstSharedPtr & car_signals_msg, + const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg); + + rclcpp::Publisher::SharedPtr pub_traffic_light_signals_; +}; + +} // namespace autoware::traffic_light + +#endif // TRAFFIC_LIGHT_CATEGORY_MERGER_NODE_HPP_