diff --git a/perception/traffic_light_visualization/CMakeLists.txt b/perception/traffic_light_visualization/CMakeLists.txt new file mode 100644 index 0000000000000..38674fabe20ba --- /dev/null +++ b/perception/traffic_light_visualization/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5) +project(traffic_light_visualization) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +########### +## Build ## +########### + +ament_auto_add_library(traffic_light_roi_visualizer_nodelet SHARED + src/traffic_light_roi_visualizer/nodelet.cpp +) + +target_link_libraries(traffic_light_roi_visualizer_nodelet + ${OpenCV_LIBRARIES} +) + +rclcpp_components_register_node(traffic_light_roi_visualizer_nodelet + PLUGIN "traffic_light::TrafficLightRoiVisualizerNodelet" + EXECUTABLE traffic_light_visualization_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_add_executable(traffic_light_map_visualizer_node + src/traffic_light_map_visualizer/node.cpp + src/traffic_light_map_visualizer/main.cpp +) + +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/traffic_light_visualization/README.md b/perception/traffic_light_visualization/README.md new file mode 100644 index 0000000000000..f675d42aaf089 --- /dev/null +++ b/perception/traffic_light_visualization/README.md @@ -0,0 +1,70 @@ +# traffic_light_visualization + +## Purpose + +The `traffic_light_visualization` is a package that includes two visualizing nodes: + +- **traffic_light_map_visualizer** is a node that shows traffic lights color status and position on rviz as markers. +- **traffic_light_roi_visualizer** is a node that draws the result of traffic light recognition nodes (traffic light status, position and classification probability) on the input image as shown in the following figure and publishes it. + +![traffic light roi visualization](./images/roi-visualization.png) + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### traffic_light_map_visualizer + +#### Input + +| Name | Type | Description | +| -------------------- | -------------------------------------------------------- | ------------------------ | +| `~/input/tl_state` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | status of traffic lights | +| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | + +#### Output + +| Name | Type | Description | +| ------------------------ | -------------------------------------- | ---------------------------------------------------- | +| `~/output/traffic_light` | `visualization_msgs::msg::MarkerArray` | marker array that indicates status of traffic lights | + +### traffic_light_roi_visualizer + +#### Input + +| Name | Type | Description | +| ----------------------------- | ---------------------------------------------------------- | ------------------------------------------------------- | +| `~/input/tl_state` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | status of traffic lights | +| `~/input/image` | `sensor_msgs::msg::Image` | the image captured by perception cameras | +| `~/input/rois` | `autoware_auto_perception_msgs::msg::TrafficLightRoiArray` | the ROIs detected by `traffic_light_ssd_fine_detector` | +| `~/input/rough/rois` (option) | `autoware_auto_perception_msgs::msg::TrafficLightRoiArray` | the ROIs detected by `traffic_light_map_based_detector` | + +#### Output + +| Name | Type | Description | +| ---------------- | ------------------------- | ---------------------- | +| `~/output/image` | `sensor_msgs::msg::Image` | output image with ROIs | + +## Parameters + +### traffic_light_map_visualizer + +None + +### traffic_light_roi_visualizer + +#### Node Parameters + +| Name | Type | Default Value | Description | +| ----------------------- | ---- | ------------- | --------------------------------------------------------------- | +| `enable_fine_detection` | bool | false | whether to visualize result of the traffic light fine detection | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/traffic_light_visualization/images/roi-visualization.png b/perception/traffic_light_visualization/images/roi-visualization.png new file mode 100644 index 0000000000000..153c7376d2810 Binary files /dev/null and b/perception/traffic_light_visualization/images/roi-visualization.png differ diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp new file mode 100644 index 0000000000000..3f82712538a76 --- /dev/null +++ b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp @@ -0,0 +1,54 @@ +// Copyright 2020 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_MAP_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace traffic_light +{ +class TrafficLightMapVisualizerNode : public rclcpp::Node +{ +public: + TrafficLightMapVisualizerNode(const std::string & node_name, const rclcpp::NodeOptions & options); + ~TrafficLightMapVisualizerNode() = default; + void trafficSignalsCallback( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr + input_traffic_signals_msg); + void binMapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_map_msg); + +private: + rclcpp::Publisher::SharedPtr light_marker_pub_; + rclcpp::Subscription::SharedPtr + tl_state_sub_; + rclcpp::Subscription::SharedPtr vector_map_sub_; + + std::vector aw_tl_reg_elems_; +}; + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp new file mode 100644 index 0000000000000..e083dfe549a03 --- /dev/null +++ b/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp @@ -0,0 +1,131 @@ +// Copyright 2020 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_ROI_VISUALIZER__NODELET_HPP_ +#define TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace traffic_light +{ +struct ClassificationResult +{ + float prob = 0.0; + std::string label; +}; + +class TrafficLightRoiVisualizerNodelet : public rclcpp::Node +{ +public: + explicit TrafficLightRoiVisualizerNodelet(const rclcpp::NodeOptions & options); + void connectCb(); + + void imageRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & + input_tl_roi_msg, + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & + input_traffic_signals_msg); + + void imageRoughRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & + input_tl_roi_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & + input_tl_rough_roi_msg, + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & + input_traffic_signals_msg); + +private: + std::map state2label_{ + // color + {autoware_auto_perception_msgs::msg::TrafficLight::RED, "red"}, + {autoware_auto_perception_msgs::msg::TrafficLight::AMBER, "yellow"}, + {autoware_auto_perception_msgs::msg::TrafficLight::GREEN, "green"}, + {autoware_auto_perception_msgs::msg::TrafficLight::WHITE, "white"}, + // shape + {autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE, "circle"}, + {autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW, "left"}, + {autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW, "right"}, + {autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW, "straight"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW, "down"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW, "down_left"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW, "down_right"}, + {autoware_auto_perception_msgs::msg::TrafficLight::CROSS, "cross"}, + // other + {autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN, "unknown"}, + }; + + bool createRect( + cv::Mat & image, const autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi, + const cv::Scalar & color); + + bool createRect( + cv::Mat & image, const autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi, + const ClassificationResult & result); + + bool getClassificationResult( + int id, const autoware_auto_perception_msgs::msg::TrafficSignalArray & traffic_signals, + ClassificationResult & result); + + bool getRoiFromId( + int id, const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & rois, + autoware_auto_perception_msgs::msg::TrafficLightRoi & correspond_roi); + + rclcpp::TimerBase::SharedPtr timer_; + image_transport::SubscriberFilter image_sub_; + message_filters::Subscriber roi_sub_; + message_filters::Subscriber + rough_roi_sub_; + message_filters::Subscriber + traffic_signals_sub_; + image_transport::Publisher image_pub_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray, + autoware_auto_perception_msgs::msg::TrafficSignalArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray, + autoware_auto_perception_msgs::msg::TrafficLightRoiArray, + autoware_auto_perception_msgs::msg::TrafficSignalArray> + SyncPolicyWithRoughRoi; + typedef message_filters::Synchronizer SyncWithRoughRoi; + std::shared_ptr sync_with_rough_roi_; + + bool enable_fine_detection_; +}; + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ diff --git a/perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml b/perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml new file mode 100644 index 0000000000000..9a9e7fc02a46d --- /dev/null +++ b/perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml b/perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml new file mode 100644 index 0000000000000..5c13f8585c774 --- /dev/null +++ b/perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/traffic_light_visualization/package.xml b/perception/traffic_light_visualization/package.xml new file mode 100644 index 0000000000000..eb00b268513ad --- /dev/null +++ b/perception/traffic_light_visualization/package.xml @@ -0,0 +1,28 @@ + + + traffic_light_visualization + 0.1.0 + The traffic_light_visualization package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_lanelet2_msgs + cv_bridge + image_transport + lanelet2_extension + message_filters + rclcpp + rclcpp_components + sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp new file mode 100644 index 0000000000000..02f7a67302945 --- /dev/null +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp @@ -0,0 +1,28 @@ +// Copyright 2020 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 +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared( + "traffic_light_map_visualizer_node", node_options); + rclcpp::spin(node); + return 0; +} diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp new file mode 100644 index 0000000000000..31f7e88a02c7f --- /dev/null +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -0,0 +1,220 @@ +// Copyright 2020 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 +#include +#include + +#include + +#include +#include + +#include +#include + +using std::placeholders::_1; + +namespace +{ +[[maybe_unused]] void setColor( + const double r, const double g, const double b, const double a, std_msgs::msg::ColorRGBA & cl) +{ + cl.r = r; + cl.g = g; + cl.b = b; + cl.a = a; +} + +bool isAttributeValue( + const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str) +{ + lanelet::Attribute attr = p.attribute(attr_str); + if (attr.value().compare(value_str) == 0) { + return true; + } + return false; +} + +bool isAttributeValue( + const lanelet::ConstLineString3d l, const std::string attr_str, const int value) +{ + lanelet::Attribute attr = l.attribute(attr_str); + if (std::stoi(attr.value()) == value) { + return true; + } + return false; +} + +void lightAsMarker( + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, + lanelet::ConstPoint3d p, visualization_msgs::msg::Marker * marker, const std::string ns, + const rclcpp::Time & current_time) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM(node_logging->get_logger(), __FUNCTION__ << ": marker is null pointer!"); + return; + } + + marker->header.frame_id = "map"; + marker->header.stamp = current_time; + marker->frame_locked = true; + marker->ns = ns; + marker->id = p.id(); + marker->lifetime = rclcpp::Duration::from_seconds(0.2); + marker->type = visualization_msgs::msg::Marker::SPHERE; + marker->pose.position.x = p.x(); + marker->pose.position.y = p.y(); + marker->pose.position.z = p.z(); + marker->pose.orientation.x = 0.0; + marker->pose.orientation.y = 0.0; + marker->pose.orientation.z = 0.0; + marker->pose.orientation.w = 1.0; + + float s = 0.3; + + marker->scale.x = s; + marker->scale.y = s; + marker->scale.z = s; + + marker->color.r = 0.0f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + marker->color.a = 0.999f; + + if (isAttributeValue(p, "color", "red")) { + marker->color.r = 1.0f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "green")) { + marker->color.r = 0.0f; + marker->color.g = 1.0f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "yellow")) { + marker->color.r = 1.0f; + marker->color.g = 1.0f; + marker->color.b = 0.0f; + } else { + marker->color.r = 1.0f; + marker->color.g = 1.0f; + marker->color.b = 1.0f; + } +} +} // namespace + +namespace traffic_light +{ +TrafficLightMapVisualizerNode::TrafficLightMapVisualizerNode( + const std::string & node_name, const rclcpp::NodeOptions & node_options) +: rclcpp::Node(node_name, node_options) +{ + light_marker_pub_ = + create_publisher("~/output/traffic_light", 1); + tl_state_sub_ = create_subscription( + "~/input/tl_state", 1, + std::bind(&TrafficLightMapVisualizerNode::trafficSignalsCallback, this, _1)); + vector_map_sub_ = create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&TrafficLightMapVisualizerNode::binMapCallback, this, _1)); +} +void TrafficLightMapVisualizerNode::trafficSignalsCallback( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr + input_traffic_signals) +{ + visualization_msgs::msg::MarkerArray output_msg; + const auto current_time = now(); + +#if 0 + for (auto tli = aw_tl_reg_elems_.begin(); tli != aw_tl_reg_elems_.end(); tli++) { + for (const auto & lsp : (*tli)->trafficLights()) { + if (lsp.isLineString()) { // traffic lights can either polygons or + // linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + for (const auto & input_traffic_signal : input_traffic_signals->signals) { + if (ls.id() != input_traffic_signal.map_primitive_id) { + continue; + } + visualization_msgs::msg::Marker marker; + std_msgs::msg::ColorRGBA color; + setColor(1.0f, 1.0f, 1.0f, 0.999f, color); + lanelet::visualization::initTrafficLightTriangleMarker(&marker, "traffic_light_triangle"); + lanelet::visualization::pushTrafficLightTriangleMarker(&marker, ls, color); + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + marker.frame_locked = true; + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + output_msg.markers.push_back(marker); + } + } + } + } +#endif + + for (auto tli = aw_tl_reg_elems_.begin(); tli != aw_tl_reg_elems_.end(); tli++) { + for (auto ls : (*tli)->lightBulbs()) { + if (!ls.hasAttribute("traffic_light_id")) { + continue; + } + for (const auto & input_traffic_signal : input_traffic_signals->signals) { + if (isAttributeValue(ls, "traffic_light_id", input_traffic_signal.map_primitive_id)) { + for (auto pt : ls) { + if (!pt.hasAttribute("color")) { + continue; + } + + for (const auto & light : input_traffic_signal.lights) { + visualization_msgs::msg::Marker marker; + if ( + isAttributeValue(pt, "color", "red") && + light.color == autoware_auto_perception_msgs::msg::TrafficLight::RED) { + lightAsMarker( + get_node_logging_interface(), pt, &marker, "traffic_light", current_time); + } else if ( // NOLINT + isAttributeValue(pt, "color", "green") && + light.color == autoware_auto_perception_msgs::msg::TrafficLight::GREEN) { + lightAsMarker( + get_node_logging_interface(), pt, &marker, "traffic_light", current_time); + + } else if ( // NOLINT + isAttributeValue(pt, "color", "yellow") && + light.color == autoware_auto_perception_msgs::msg::TrafficLight::AMBER) { + lightAsMarker( + get_node_logging_interface(), pt, &marker, "traffic_light", current_time); + } else { + continue; + } + output_msg.markers.push_back(marker); + } + } + } + } + } + } + + light_marker_pub_->publish(output_msg); +} + +void TrafficLightMapVisualizerNode::binMapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_map_msg) +{ + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::fromBinMsg(*input_map_msg, viz_lanelet_map); + RCLCPP_INFO(get_logger(), "Map is loaded\n"); + + // get lanelets etc to visualize + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + aw_tl_reg_elems_ = lanelet::utils::query::autowareTrafficLights(all_lanelets); +} +} // namespace traffic_light diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp new file mode 100644 index 0000000000000..0f2d18ae97077 --- /dev/null +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -0,0 +1,223 @@ +// Copyright 2020 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 +#include +#include + +#include +#include +#include + +namespace traffic_light +{ +TrafficLightRoiVisualizerNodelet::TrafficLightRoiVisualizerNodelet( + const rclcpp::NodeOptions & options) +: Node("traffic_light_roi_visualizer_node", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + using std::placeholders::_4; + enable_fine_detection_ = this->declare_parameter("enable_fine_detection", false); + + if (enable_fine_detection_) { + sync_with_rough_roi_.reset(new SyncWithRoughRoi( + SyncPolicyWithRoughRoi(10), image_sub_, roi_sub_, rough_roi_sub_, traffic_signals_sub_)); + sync_with_rough_roi_->registerCallback( + std::bind(&TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback, this, _1, _2, _3, _4)); + } else { + sync_.reset(new Sync(SyncPolicy(10), image_sub_, roi_sub_, traffic_signals_sub_)); + sync_->registerCallback( + std::bind(&TrafficLightRoiVisualizerNodelet::imageRoiCallback, this, _1, _2, _3)); + } + + auto timer_callback = std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this); + const auto period_s = 0.1; + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + image_pub_ = + image_transport::create_publisher(this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile()); +} + +void TrafficLightRoiVisualizerNodelet::connectCb() +{ + if (image_pub_.getNumSubscribers() == 0) { + image_sub_.unsubscribe(); + traffic_signals_sub_.unsubscribe(); + roi_sub_.unsubscribe(); + if (enable_fine_detection_) { + rough_roi_sub_.unsubscribe(); + } + } else if (!image_sub_.getSubscriber()) { + image_sub_.subscribe(this, "~/input/image", "raw", rmw_qos_profile_sensor_data); + roi_sub_.subscribe(this, "~/input/rois", rclcpp::QoS{1}.get_rmw_qos_profile()); + traffic_signals_sub_.subscribe( + this, "~/input/traffic_signals", rclcpp::QoS{1}.get_rmw_qos_profile()); + if (enable_fine_detection_) { + rough_roi_sub_.subscribe(this, "~/input/rough/rois", rclcpp::QoS{1}.get_rmw_qos_profile()); + } + } +} + +bool TrafficLightRoiVisualizerNodelet::createRect( + cv::Mat & image, const autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi, + const cv::Scalar & color) +{ + cv::rectangle( + image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), + cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height), + color, 3); + cv::putText( + image, std::to_string(tl_roi.id), cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), + cv::FONT_HERSHEY_COMPLEX, 1.0, color, 1, CV_AA); + return true; +} + +bool TrafficLightRoiVisualizerNodelet::createRect( + cv::Mat & image, const autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi, + const ClassificationResult & result) +{ + cv::Scalar color; + if (result.label.find("red") != std::string::npos) { + color = cv::Scalar{255, 0, 0}; + } else if (result.label.find("yellow") != std::string::npos) { + color = cv::Scalar{0, 255, 0}; + } else if (result.label.find("green") != std::string::npos) { + color = cv::Scalar{0, 0, 255}; + } else { + color = cv::Scalar{255, 255, 255}; + } + + cv::rectangle( + image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), + cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height), + color, 3); + + int offset = 40; + cv::putText( + image, std::to_string(result.prob), + cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset - (offset * 0)), cv::FONT_HERSHEY_COMPLEX, + 1.1, color, 3); + + cv::putText( + image, result.label, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset - (offset * 1)), + cv::FONT_HERSHEY_COMPLEX, 1.1, color, 2); + + return true; +} + +void TrafficLightRoiVisualizerNodelet::imageRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_tl_roi_msg, + [[maybe_unused]] const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & + input_traffic_signals_msg) +{ + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + for (auto tl_roi : input_tl_roi_msg->rois) { + createRect(cv_ptr->image, tl_roi, cv::Scalar(0, 255, 0)); + } + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR( + get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + } + image_pub_.publish(cv_ptr->toImageMsg()); +} + +bool TrafficLightRoiVisualizerNodelet::getClassificationResult( + int id, const autoware_auto_perception_msgs::msg::TrafficSignalArray & traffic_signals, + ClassificationResult & result) +{ + bool has_correspond_traffic_signal = false; + for (const auto & traffic_signal : traffic_signals.signals) { + if (id != traffic_signal.map_primitive_id) { + continue; + } + has_correspond_traffic_signal = true; + for (size_t i = 0; i < traffic_signal.lights.size(); i++) { + auto light = traffic_signal.lights.at(i); + // all lamp confidence are the same + result.prob = light.confidence; + result.label += (state2label_[light.color] + "-" + state2label_[light.shape]); + if (i < traffic_signal.lights.size() - 1) { + result.label += ","; + } + } + } + return has_correspond_traffic_signal; +} + +bool TrafficLightRoiVisualizerNodelet::getRoiFromId( + int id, const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & rois, + autoware_auto_perception_msgs::msg::TrafficLightRoi & correspond_roi) +{ + for (const auto roi : rois->rois) { + if (roi.id == id) { + correspond_roi = roi; + return true; + } + } + return false; +} + +void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_tl_roi_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & + input_tl_rough_roi_msg, + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & + input_traffic_signals_msg) +{ + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) { + // visualize rough roi + createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0)); + + ClassificationResult result; + bool has_correspond_traffic_signal = + getClassificationResult(tl_rough_roi.id, *input_traffic_signals_msg, result); + autoware_auto_perception_msgs::msg::TrafficLightRoi tl_roi; + bool has_correspond_roi = getRoiFromId(tl_rough_roi.id, input_tl_roi_msg, tl_roi); + + if (has_correspond_roi && has_correspond_traffic_signal) { + // has fine detection and classification results + createRect(cv_ptr->image, tl_roi, result); + } else if (has_correspond_roi && !has_correspond_traffic_signal) { + // has fine detection result and does not have classification result + createRect(cv_ptr->image, tl_roi, cv::Scalar(255, 255, 255)); + } else if (!has_correspond_roi && has_correspond_traffic_signal) { + // does not have fine detection result and has classification result + createRect(cv_ptr->image, tl_rough_roi, result); + } else { + } + } + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR( + get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + } + image_pub_.publish(cv_ptr->toImageMsg()); +} + +} // namespace traffic_light + +RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightRoiVisualizerNodelet)