diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml index 0d35cbca78392..ca41ad8b3147e 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml @@ -63,7 +63,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index bafc42a659100..abce939731e8a 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -130,6 +130,7 @@ def create_parameter_dict(*args): remappings=[ ("input/car_signals", "classified/car/traffic_signals"), ("input/pedestrian_signals", "classified/pedestrian/traffic_signals"), + ("input/expect_rois", f"/perception/traffic_light_recognition/{namespace}/detection/expect/rois"), ("output/traffic_light_signals", "traffic_signals"), ], extra_arguments=[ diff --git a/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.cpp b/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.cpp index 9fc109c5ad3c0..9ff904b8dcec5 100644 --- a/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.cpp +++ b/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.cpp @@ -14,6 +14,7 @@ #include "traffic_light_signals_merger_node.hpp" +#include #include #include #include @@ -28,19 +29,30 @@ TrafficLightSignalsMergerNode::TrafficLightSignalsMergerNode( 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_) + expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), car_signal_sub_, pedestrian_signal_sub_, expected_rois_sub_) { using std::placeholders::_1; using std::placeholders::_2; - sync_.registerCallback(std::bind(&TrafficLightSignalsMergerNode::signalsCallback, this, _1, _2)); + using std::placeholders::_3; + sync_.registerCallback( + std::bind(&TrafficLightSignalsMergerNode::signalsCallback, this, _1, _2, _3)); pub_traffic_light_signals_ = create_publisher("output/traffic_light_signals", rclcpp::QoS{1}); } void TrafficLightSignalsMergerNode::signalsCallback( const TrafficLightArray::ConstSharedPtr & car_signals_msg, - const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg) + const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg, + const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) { + std::map expected_rois_map; + for (const auto & roi : expected_rois_msg->rois) { + expected_rois_map[roi.traffic_light_id].traffic_light_id = roi.traffic_light_id; + expected_rois_map[roi.traffic_light_id].roi = roi.roi; + expected_rois_map[roi.traffic_light_id].traffic_light_type = roi.traffic_light_type; + } + TrafficLightArray output; output.header = car_signals_msg->header; output.signals.insert( @@ -48,6 +60,22 @@ void TrafficLightSignalsMergerNode::signalsCallback( output.signals.insert( output.signals.end(), pedestrian_signals_msg->signals.begin(), pedestrian_signals_msg->signals.end()); + for (auto & signal : output.signals) { + // remove expected_rois which are already in signals + if (expected_rois_map.find(signal.traffic_light_id) != expected_rois_map.end()) { + expected_rois_map.erase(signal.traffic_light_id); + } + } + for (const auto & roi : expected_rois_map) { + TrafficLight signal; + signal.traffic_light_id = roi.first; + signal.traffic_light_type = roi.second.traffic_light_type; + signal.elements.resize(1); + signal.elements[0].shape = TrafficLightElement::UNKNOWN; + signal.elements[0].color = TrafficLightElement::UNKNOWN; + signal.elements[0].confidence = 1.0; + output.signals.push_back(signal); + } pub_traffic_light_signals_->publish(output); } diff --git a/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.hpp b/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.hpp index c64b2dffd2756..7c195d1dbef83 100644 --- a/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.hpp +++ b/perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.hpp @@ -21,7 +21,11 @@ #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" +#include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_array.hpp" +#include "tier4_perception_msgs/msg/traffic_light_element.hpp" +#include "tier4_perception_msgs/msg/traffic_light_roi.hpp" +#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp" #include #include @@ -30,7 +34,11 @@ namespace autoware::traffic_light { +using tier4_perception_msgs::msg::TrafficLight; using tier4_perception_msgs::msg::TrafficLightArray; +using tier4_perception_msgs::msg::TrafficLightElement; +using tier4_perception_msgs::msg::TrafficLightRoi; +using tier4_perception_msgs::msg::TrafficLightRoiArray; class TrafficLightSignalsMergerNode : public rclcpp::Node { @@ -44,14 +52,17 @@ class TrafficLightSignalsMergerNode : public rclcpp::Node message_filters::Subscriber car_signal_sub_; message_filters::Subscriber pedestrian_signal_sub_; + message_filters::Subscriber expected_rois_sub_; - typedef message_filters::sync_policies::ApproximateTime + typedef message_filters::sync_policies::ApproximateTime< + TrafficLightArray, TrafficLightArray, TrafficLightRoiArray> SyncPolicy; message_filters::Synchronizer sync_; void signalsCallback( const TrafficLightArray::ConstSharedPtr & car_signals_msg, - const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg); + const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg, + const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg); rclcpp::Publisher::SharedPtr pub_traffic_light_signals_; };