[traffic_light_ssd_fine_detector] port to ROS2 (#113)

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

perception/traffic_light_ssd_fine_detector/CMakeLists.txt

@@ -1,21 +1,20 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 
-add_compile_options(-std=c++14)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
 project(traffic_light_ssd_fine_detector)
 
 option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF)
 
-find_package(catkin REQUIRED COMPONENTS
-  cv_bridge
-  roscpp
-  image_transport
-  message_filters
-  sensor_msgs
-  std_msgs
-  autoware_perception_msgs
-  nodelet
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
 find_package(OpenCV REQUIRED)
 
@@ -102,22 +101,14 @@ endif()
 
 if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   include_directories(
-    include
-    ${Opencv_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
     ${CUDA_INCLUDE_DIRS}
-    ${catkin_INCLUDE_DIRS}
-  )
-
-  catkin_package(
-    INCLUDE_DIRS include
-    LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS
   )
 
   set(CMAKE_CXX_FLAGS "-O2 -Wall -Wunused-variable ${CMAKE_CXX_FLAGS} -fpic -std=c++11 -pthread")
 
   ### ssd ###
-  add_library(ssd
+  ament_auto_add_library(ssd 
     lib/src/trt_ssd.cpp
   )
 
@@ -135,67 +126,26 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     ${CUDNN_LIBRARY}
   )
 
-  add_library(traffic_light_ssd_fine_detector_nodelet SHARED
+  ament_auto_add_library(traffic_light_ssd_fine_detector_nodelet SHARED
     src/nodelet.cpp
   )
 
-  add_dependencies(traffic_light_ssd_fine_detector_nodelet
-    ${catkin_EXPORTED_TARGETS}
-  )
-
   target_link_libraries(traffic_light_ssd_fine_detector_nodelet
-    ${catkin_LIBRARIES}
-    ${OpenCV_LIBS}
+    ${OpenCV_LIB}
     ssd
   )
 
-  add_executable(traffic_light_ssd_fine_detector_node
-    src/node.cpp
+  rclcpp_components_register_node(traffic_light_ssd_fine_detector_nodelet
+    PLUGIN "traffic_light::TrafficLightSSDFineDetectorNodelet"
+    EXECUTABLE traffic_light_ssd_fine_detector_node
   )
 
-  target_link_libraries(traffic_light_ssd_fine_detector_node
-    traffic_light_ssd_fine_detector_nodelet
-    ${catkin_LIBRARIES}
-    ${OpenCV_LIBS}
-    ssd
-  )
-
-  add_dependencies(traffic_light_ssd_fine_detector_node
-    ${catkin_EXPORTED_TARGETS}
-  )
-
-  install(TARGETS
-    traffic_light_ssd_fine_detector_node
-    traffic_light_ssd_fine_detector_nodelet
-    ssd
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-
-  install(
-    FILES
-    traffic_light_ssd_fine_detector.xml
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  )
-
-  install(
-    DIRECTORY
-      launch
-      data
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  ament_auto_package(INSTALL_TO_SHARE
+    data
+    launch
   )
 
 else()
-  find_package(catkin REQUIRED)
-  catkin_package()
   message("TrafficLightSSDFineDetector won't be built, CUDA and/or TensorRT were not found.")
-
-  install(
-    DIRECTORY
-      launch
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  )
-
 endif()
 

perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp

@@ -22,29 +22,26 @@
 #include <mutex>
 #include <string>
 
-#include <opencv/cv.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
 #include "trt_ssd.h"
 
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/time_synchronizer.h>
-#include <nodelet/nodelet.h>
-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/image_encodings.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Header.h>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/header.hpp>
 
-#include <autoware_perception_msgs/TrafficLightRoiArray.h>
+#include <autoware_perception_msgs/msg/traffic_light_roi_array.hpp>
 
 typedef struct Detection
 {
@@ -53,45 +50,45 @@ typedef struct Detection
 
 namespace traffic_light
 {
-class TrafficLightSSDFineDetectorNodelet : public nodelet::Nodelet
+class TrafficLightSSDFineDetectorNodelet : public rclcpp::Node
 {
 public:
-  virtual void onInit();
+  TrafficLightSSDFineDetectorNodelet(const rclcpp::NodeOptions & options);
   void connectCb();
   void callback(
-    const sensor_msgs::Image::ConstPtr & image_msg,
-    const autoware_perception_msgs::TrafficLightRoiArray::ConstPtr & traffic_light_roi_msg);
+    const sensor_msgs::msg::Image::ConstSharedPtr image_msg,
+    const autoware_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr traffic_light_roi_msg);
 
 private:
   bool cvMat2CnnInput(
     const std::vector<cv::Mat> & in_imgs, const int num_rois, std::vector<float> & data);
   bool cnnOutput2BoxDetection(
     const float * scores, const float * boxes, const int tlr_id,
     const std::vector<cv::Mat> & in_imgs, const int num_rois, std::vector<Detection> & detctions);
-  bool rosMsg2CvMat(const sensor_msgs::Image::ConstPtr & image_msg, cv::Mat & image);
+  bool rosMsg2CvMat(const sensor_msgs::msg::Image::ConstSharedPtr image_msg, cv::Mat & image);
   bool fitInFrame(cv::Point & lt, cv::Point & rb, const cv::Size & size);
   void cvRect2TlRoiMsg(
-    const cv::Rect & rect, const int32_t id, autoware_perception_msgs::TrafficLightRoi & tl_roi);
+    const cv::Rect & rect, const int32_t id, autoware_perception_msgs::msg::TrafficLightRoi & tl_roi);
   bool readLabelFile(std::string filepath, std::vector<std::string> & labels);
   bool getTlrIdFromLabel(const std::vector<std::string> & labels, int & tlr_id);
 
   // variables
-  ros::NodeHandle nh_, pnh_;
   std::shared_ptr<image_transport::ImageTransport> image_transport_;
   image_transport::SubscriberFilter image_sub_;
-  message_filters::Subscriber<autoware_perception_msgs::TrafficLightRoiArray> roi_sub_;
+  message_filters::Subscriber<autoware_perception_msgs::msg::TrafficLightRoiArray> roi_sub_;
   std::mutex connect_mutex_;
-  ros::Publisher output_roi_pub_;
-  ros::Publisher exe_time_pub_;
+  rclcpp::Publisher<autoware_perception_msgs::msg::TrafficLightRoiArray>::SharedPtr output_roi_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr exe_time_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
 
   typedef message_filters::sync_policies::ExactTime<
-    sensor_msgs::Image, autoware_perception_msgs::TrafficLightRoiArray>
+    sensor_msgs::msg::Image, autoware_perception_msgs::msg::TrafficLightRoiArray>
     SyncPolicy;
   typedef message_filters::Synchronizer<SyncPolicy> Sync;
   std::shared_ptr<Sync> sync_;
 
   typedef message_filters::sync_policies::ApproximateTime<
-    sensor_msgs::Image, autoware_perception_msgs::TrafficLightRoiArray>
+    sensor_msgs::msg::Image, autoware_perception_msgs::msg::TrafficLightRoiArray>
     ApproximateSyncPolicy;
   typedef message_filters::Synchronizer<ApproximateSyncPolicy> ApproximateSync;
   std::shared_ptr<ApproximateSync> approximate_sync_;

perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml

@@ -1,7 +1,7 @@
 <launch>
 
-  <arg name="onnx_file" default="$(find traffic_light_ssd_fine_detector)/data/mb2-ssd-lite-tlr.onnx"/>
-  <arg name="label_file" default="$(find traffic_light_ssd_fine_detector)/data/voc_labels_tl.txt"/>
+  <arg name="onnx_file" default="$(find-pkg-share traffic_light_ssd_fine_detector)/data/mb2-ssd-lite-tlr.onnx"/>
+  <arg name="label_file" default="$(find-pkg-share traffic_light_ssd_fine_detector)/data/voc_labels_tl.txt"/>
   <arg name="mode" default="FP32"/>
   <arg name="input/image" default="/image_raw"/>
   <arg name="input/rois" default="/traffic_light_map_based_detector/output/rois" />
@@ -14,24 +14,24 @@
   <arg name="save_rough_roi_image" default="false"/>
   <arg name="manager" default="traffic_light_recognition_nodelet_manager"/>
 
-  <node pkg="nodelet" type="nodelet" name="traffic_light_ssd_fine_detector" args="load traffic_light/traffic_light_ssd_fine_detector $(arg manager)">
-    <remap from="input/image" to="$(arg input/image)" />
-    <remap from="~input/rois" to="$(arg input/rois)" />
-    <remap from="~output/rois" to="$(arg output/rois)" />
-    <param name="onnx_file" type="str" value="$(arg onnx_file)"/>
-    <param name="label_file" type="str" value="$(arg label_file)"/>
-    <param name="mode" type="str" value="$(arg mode)"/>
-    <param name="score_thresh" value="$(arg score_thresh)"/>
-    <param name="max_batch_size" value="$(arg max_batch_size)"/>
-    <param name="approximate_sync" value="$(arg approximate_sync)"/>
-    <rosparam param="mean" subst_value="True">$(arg mean)</rosparam>
-    <rosparam param="std" subst_value="True">$(arg std)</rosparam>
+  <node pkg="traffic_light_ssd_fine_detector" exec="traffic_light_ssd_fine_detector_node" name="traffic_light_ssd_fine_detector">
+    <remap from="input/image" to="$(var input/image)" />
+    <remap from="input/rois" to="$(var input/rois)" />
+    <remap from="output/rois" to="$(var output/rois)" />
+    <param name="onnx_file" type="str" value="$(var onnx_file)"/>
+    <param name="label_file" type="str" value="$(var label_file)"/>
+    <param name="mode" type="str" value="$(var mode)"/>
+    <param name="score_thresh" value="$(var score_thresh)"/>
+    <param name="max_batch_size" value="$(var max_batch_size)"/>
+    <param name="approximate_sync" value="$(var approximate_sync)"/>
+    <param name="mean" value="$(var mean)" />
+    <param name="std" value="$(var std)" />
   </node>
 
-  <node if="$(eval save_rough_roi_image)" pkg="roi_image_saver" type="traffic_light_roi_image_saver_node" name="$(anon traffic_light_roi_image_saver)" output="screen">
-    <remap from="~input/image" to="$(arg input/image)"/>
-    <remap from="~input/rois" to="$(arg input/rois)"/>
-    <param name="save_dir" value="$(find roi_image_saver)/train_image"/>
+  <node if="$(var save_rough_roi_image)" pkg="roi_image_saver" exec="traffic_light_roi_image_saver_node" name="$(anon traffic_light_roi_image_saver)" output="screen">
+    <remap from="input/image" to="$(var input/image)"/>
+    <remap from="input/rois" to="$(var input/rois)"/>
+    <param name="save_dir" value="$(find-pkg-share roi_image_saver)/train_image"/>
     <param name="save_rate" value="1.0"/>
   </node>
 </launch>

perception/traffic_light_ssd_fine_detector/package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>traffic_light_ssd_fine_detector</name>
   <version>0.1.0</version>
   <description>The traffic_light_ssd_fine_detector package</description>
@@ -15,30 +16,18 @@
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>Apache 2</license>
 
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>autoware_perception_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>message_filters</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_export_depend>autoware_perception_msgs</build_export_depend>
-  <build_export_depend>cv_bridge</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
-  <build_export_depend>message_filters</build_export_depend>
-  <build_export_depend>nodelet</build_export_depend>
-  <exec_depend>autoware_perception_msgs</exec_depend>
-  <exec_depend>cv_bridge</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>message_filters</exec_depend>
-  <exec_depend>nodelet</exec_depend>
-
+  <depend>autoware_perception_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <nodelet plugin="${prefix}/traffic_light_ssd_fine_detector.xml" />
+    <build_type>ament_cmake</build_type>
   </export>
 </package>