feat: add traffic light visualization package (autowarefoundation#114)

* release v0.4.0

* fixed bug (autowarefoundation#717)

* Add nodelets of tlr nodes (autowarefoundation#715)

* Add classifier nodelet

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Replace boost::shared_ptr into std::shared_ptr

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Add lock guard

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Add detetcor nodelet

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Integrate main into node

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Add SubscriberStatusCallback

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* add image_transport_decompresser nodelet

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* Add visualizer nodelet

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* fixed bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* Fix plugin name

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Launch nodelet

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* Fix classifier constructor

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* add decompresser node

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix typo

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fixed bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fixed bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* cosmetic change

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add param

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* Fix build warning

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* change rgb

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* change rgb

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit e884716313c52a9392fe58f3e6a1c95e7515725e.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Port traffic light visualization (autowarefoundation#121)

* Port

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Fix package.xml

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Removed unused mutex

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* support traffic_light_state in traffi_light_visualization (autowarefoundation#1115) (autowarefoundation#339)

* support traffic_light_state in traffi_light_visualization

* support unsynced detection and classification result

* apply clang-format

Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>

* add use_sim-time option (autowarefoundation#454)

* Sync public repo (autowarefoundation#1228)

* [simple_planning_simulator] add readme (autowarefoundation#424)

* add readme of simple_planning_simulator

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update simulator/simple_planning_simulator/README.md

* set transit_margin_time to intersect. planner (autowarefoundation#460)

* Fix pose2twist (autowarefoundation#462)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Ros2 vehicle info param server (autowarefoundation#447)

* add vehicle_info_param_server

* update vehicle info

* apply format

* fix bug

* skip unnecessary search

* delete vehicle param file

* fix bug

* Ros2 fix topic name part2 (autowarefoundation#425)

* Fix topic name of traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_visualization

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_map_based_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_recognition

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix issues in hdd_reader (autowarefoundation#466)

* Fix some issues detected by Coverity Scan and Clang-Tidy

* Update launch command

* Add more `close(new_sock)`

* Simplify the definitions of struct

* fix: re-construct laneletMapLayer for reindex RTree (autowarefoundation#463)

* Rviz overlay render fix (autowarefoundation#461)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* back to RTD as superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Rviz overlay render in update (autowarefoundation#465)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* removed unnecessary includes and some dead code

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* restored RTD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Use sensor data qos for traffic light recognition (autowarefoundation#1440)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add pre-commit (autowarefoundation#1560)

* add pre-commit

* add pre-commit-config

* add additional settings for private repository

* use default pre-commit-config

* update pre-commit setting

* Ignore whitespace for line breaks in markdown

* Update .github/workflows/pre-commit.yml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* exclude svg

* remove pretty-format-json

* add double-quote-string-fixer

* consider COLCON_IGNORE file when seaching modified package

* format file

* pre-commit fixes

* Update pre-commit.yml

* Update .pre-commit-config.yaml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Porting traffic light viz (autowarefoundation#1284)

* Feature/traffic light viz (autowarefoundation#1001)

* add tl map viz

* bug fix

* update map visualizer

* add launch

* add install in cmake

* remove unused file

* fix build error

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix topic name and qos

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Replace deprecated duration api

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>

* suppress warnings for traffic light classifier (autowarefoundation#1762)

* add Werror

* fix type

* fix unused

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* [traffic_light_visualization] support autoware auto msgs  (autowarefoundation#517)

* doc traffic light visualizer (autowarefoundation#628)

* add readme

* run pre-commit

* add purpose

* fix typo

* run pre-commit

* remove reference

* update descriptions

* Update traffic light topic name (autowarefoundation#729)

* Update traffic light topic name

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Update traffic light topic name in perception

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com>
Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>

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;
+}