feat: add traffic light visualization

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
+)

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

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 <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+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<visualization_msgs::msg::MarkerArray>::SharedPtr light_marker_pub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>::SharedPtr
+    tl_state_sub_;
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr vector_map_sub_;
+
+  std::vector<lanelet::AutowareTrafficLightConstPtr> aw_tl_reg_elems_;
+};
+
+}  // namespace traffic_light
+
+#endif  // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_

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 <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/traffic_light_roi_array.hpp>
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <opencv2/imgproc/imgproc_c.h>
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+
+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<int, std::string> 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<autoware_auto_perception_msgs::msg::TrafficLightRoiArray> roi_sub_;
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::TrafficLightRoiArray>
+    rough_roi_sub_;
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::TrafficSignalArray>
+    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<SyncPolicy> Sync;
+  std::shared_ptr<Sync> 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<SyncPolicyWithRoughRoi> SyncWithRoughRoi;
+  std::shared_ptr<SyncWithRoughRoi> sync_with_rough_roi_;
+
+  bool enable_fine_detection_;
+};
+
+}  // namespace traffic_light
+
+#endif  // TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_

perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="input/tl_state" default="traffic_signals"/>
+  <arg name="input/vector_map" default="/map/vector_map"/>
+  <arg name="output/traffic_light" default="traffic_signals/markers"/>
+
+  <node pkg="traffic_light_visualization" exec="traffic_light_map_visualizer_node" name="traffic_light_map_visualizer">
+    <remap from="~/input/tl_state" to="$(var input/tl_state)"/>
+    <remap from="~/input/vector_map" to="$(var input/vector_map)"/>
+    <remap from="~/output/traffic_light" to="$(var output/traffic_light)"/>
+  </node>
+</launch>

perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input/image" default="/camera/image_raw"/>
+  <arg name="input/rois" default="~/input/rois"/>
+  <arg name="input/rough/rois" default="~/rough/rois"/>
+  <arg name="input/traffic_signals" default="~/input/traffic_signals"/>
+  <arg name="output/image" default="~/debug/rois"/>
+  <arg name="enable_fine_detection" default="false"/>
+
+  <node pkg="traffic_light_visualization" exec="traffic_light_visualization_node" name="traffic_light_visualization">
+    <remap from="~/input/image" to="$(var input/image)"/>
+    <remap from="~/input/rois" to="$(var input/rois)"/>
+    <remap from="~/input/rough/rois" to="$(var input/rough/rois)"/>
+    <remap from="~/input/traffic_signals" to="$(var input/traffic_signals)"/>
+    <remap from="~/output/image" to="$(var output/image)"/>
+    <param name="enable_fine_detection" value="$(var enable_fine_detection)"/>
+  </node>
+</launch>

perception/traffic_light_visualization/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traffic_light_visualization</name>
+  <version>0.1.0</version>
+  <description>The traffic_light_visualization package</description>
+
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_lanelet2_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>message_filters</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</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/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 <rclcpp/rclcpp.hpp>
+#include <traffic_light_map_visualizer/node.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  auto node = std::make_shared<traffic_light::TrafficLightMapVisualizerNode>(
+    "traffic_light_map_visualizer_node", node_options);
+  rclcpp::spin(node);
+  return 0;
+}