Add nodelets of tlr nodes (#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>
autowarefoundation · taikitanaka3 · Dec 3, 2021 · Sep 18, 2020 · Jul 21, 2020 · Jul 30, 2020
commit 4a02612acb35b5e631c882904931bfc405fc75a3
diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt
@@ -92,6 +92,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   message_filters
   dynamic_reconfigure
+  nodelet
   )
 
 generate_dynamic_reconfigure_options(cfg/HSVFilter.cfg)
@@ -131,30 +132,55 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   add_dependencies(libutils
     ${catkin_EXPORTED_TARGETS})
 
-  add_executable(traffic_light_classifier_node
+  add_library(traffic_light_classifier_nodelet SHARED
     src/color_classifier.cpp
     src/cnn_classifier.cpp
+    src/nodelet.cpp
+    )
+
+  add_dependencies(traffic_light_classifier_nodelet
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+    )
+
+  target_link_libraries(traffic_light_classifier_nodelet
+    libutils
+    ${catkin_LIBRARIES}
+    ${OpenCV_LIBRARIES}
+    )
+
+  add_executable(traffic_light_classifier_node
     src/node.cpp
-    src/main.cpp
     )
 
-  add_dependencies(traffic_light_classifier_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}
+  add_dependencies(traffic_light_classifier_node
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
     )
+
   target_link_libraries(traffic_light_classifier_node
     libutils
+    traffic_light_classifier_nodelet
     ${catkin_LIBRARIES}
     ${OpenCV_LIBRARIES}
     )
 
   install(
     TARGETS
     traffic_light_classifier_node
+    traffic_light_classifier_nodelet
     libutils
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
     )
 
+  install(
+    FILES
+    traffic_light_classifier.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    )
+
   install(
     DIRECTORY
     launch
@@ -177,27 +203,53 @@ else()
     CATKIN_DEPENDS
     )
 
-  add_executable(traffic_light_classifier_node
+  add_library(traffic_light_classifier_nodelet SHARED
     src/color_classifier.cpp
+    src/nodelet.cpp
+    )
+
+  add_dependencies(traffic_light_classifier_nodelet
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+    )
+
+  target_link_libraries(traffic_light_classifier_nodelet
+    libutils
+    ${catkin_LIBRARIES}
+    ${OpenCV_LIBRARIES}
+    )
+
+  add_executable(traffic_light_classifier_node
     src/node.cpp
-    src/main.cpp
     )
 
-  add_dependencies(traffic_light_classifier_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}
+  add_dependencies(traffic_light_classifier_node
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
     )
+
   target_link_libraries(traffic_light_classifier_node
+    libutils
+    traffic_light_classifier_nodelet
     ${catkin_LIBRARIES}
     ${OpenCV_LIBRARIES}
     )
 
   install(
     TARGETS
     traffic_light_classifier_node
+    traffic_light_classifier_nodelet
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
     )
 
+  install(
+    FILES
+    traffic_light_classifier.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    )
+
   install(
     DIRECTORY
     launch

diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp
@@ -34,8 +34,7 @@ namespace traffic_light
 class CNNClassifier : public ClassifierInterface
 {
 public:
-  CNNClassifier();
-  ~CNNClassifier(){};
+  explicit CNNClassifier(const ros::NodeHandle & nh, const ros::NodeHandle & pnh);
 
   bool getLampState(
     const cv::Mat & input_image,

diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp
@@ -31,8 +31,7 @@ namespace traffic_light
 class ColorClassifier : public ClassifierInterface
 {
 public:
-  ColorClassifier();
-  ~ColorClassifier(){};
+  explicit ColorClassifier(const ros::NodeHandle & nh, const ros::NodeHandle & pnh);
 
   bool getLampState(
     const cv::Mat & input_image,

diff --git a/...include/traffic_light_classifier/node.hpp → ...lude/traffic_light_classifier/nodelet.hpp b/...include/traffic_light_classifier/node.hpp → ...lude/traffic_light_classifier/nodelet.hpp
@@ -26,6 +26,8 @@
 #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 <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
@@ -43,14 +45,18 @@
 #include <opencv2/highgui/highgui.hpp>
 
 #include <memory>
+#include <mutex>
 
 namespace traffic_light
 {
-class TrafficLightClassifierNode
+class TrafficLightClassifierNodelet : public nodelet::Nodelet
 {
 public:
-  TrafficLightClassifierNode();
-  virtual ~TrafficLightClassifierNode(){};
+  virtual void onInit();
+  void connectCb();
+  void imageRoiCallback(
+    const sensor_msgs::ImageConstPtr & input_image_msg,
+    const autoware_perception_msgs::TrafficLightRoiArrayConstPtr & input_rois_msg);
 
 private:
   enum ClassifierType {
@@ -60,25 +66,23 @@ class TrafficLightClassifierNode
 
   ros::NodeHandle nh_;
   ros::NodeHandle pnh_;
-  image_transport::ImageTransport image_transport_;
+  std::shared_ptr<image_transport::ImageTransport> image_transport_;
   image_transport::SubscriberFilter image_sub_;
   message_filters::Subscriber<autoware_perception_msgs::TrafficLightRoiArray> roi_sub_;
   typedef message_filters::sync_policies::ExactTime<
     sensor_msgs::Image, autoware_perception_msgs::TrafficLightRoiArray>
     SyncPolicy;
   typedef message_filters::Synchronizer<SyncPolicy> Sync;
-  Sync sync_;
+  std::shared_ptr<Sync> sync_;
   typedef message_filters::sync_policies::ApproximateTime<
     sensor_msgs::Image, autoware_perception_msgs::TrafficLightRoiArray>
     ApproximateSyncPolicy;
   typedef message_filters::Synchronizer<ApproximateSyncPolicy> ApproximateSync;
-  ApproximateSync approximate_sync_;
+  std::shared_ptr<ApproximateSync> approximate_sync_;
   bool is_approximate_sync_;
+  std::mutex connect_mutex_;
   ros::Publisher tl_states_pub_;
   std::shared_ptr<ClassifierInterface> classifier_ptr_;
-  void imageRoiCallback(
-    const sensor_msgs::ImageConstPtr & input_image_msg,
-    const autoware_perception_msgs::TrafficLightRoiArrayConstPtr & input_rois_msg);
 };
 
 }  // namespace traffic_light
diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch
@@ -2,13 +2,14 @@
   <arg name="input/image" default="image_raw" />
   <arg name="input/rois" default="rois" />
   <arg name="output/traffic_light_states" default="traffic_light_states" />
+  <arg name="manager" default="traffic_light_recognition_nodelet_manager" />
 
   <arg name="use_gpu" default="false" />
   <!-- classifier_type {hsv_filter: 0, cnn: 1} -->
   <arg name="classifier_type" default="1" if="$(arg use_gpu)"/>
   <arg name="classifier_type" default="0" unless="$(arg use_gpu)"/>
 
-  <node name="traffic_light_classifier" pkg="traffic_light_classifier" type="traffic_light_classifier_node" output="screen">
+  <node pkg="nodelet" type="nodelet" name="traffic_light_classifier" args="load traffic_light/traffic_light_classifier $(arg manager)">
     <remap from="~input/image" to="$(arg input/image)" />
     <remap from="~input/rois" to="$(arg input/rois)" />
     <remap from="~output/traffic_light_states" to="$(arg output/traffic_light_states)" />

diff --git a/perception/traffic_light_classifier/package.xml b/perception/traffic_light_classifier/package.xml
@@ -48,25 +48,27 @@
   <build_depend>image_transport</build_depend>
   <build_depend>message_filters</build_depend>
   <build_depend>dynamic_reconfigure</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>image_transport</build_export_depend>
   <build_export_depend>message_filters</build_export_depend>
   <build_export_depend>dynamic_reconfigure</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>image_transport</exec_depend>
   <exec_depend>message_filters</exec_depend>
   <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>nodelet</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
-
+    <nodelet plugin="${prefix}/traffic_light_classifier.xml" />
   </export>
 </package>
diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/cnn_classifier.cpp
@@ -19,7 +19,8 @@
 
 namespace traffic_light
 {
-CNNClassifier::CNNClassifier() : nh_(""), pnh_("~"), image_transport_(pnh_)
+CNNClassifier::CNNClassifier(const ros::NodeHandle & nh, const ros::NodeHandle & pnh)
+: nh_(nh), pnh_(pnh), image_transport_(pnh_)
 {
   image_pub_ = image_transport_.advertise("output/debug/image", 1);
 
@@ -119,7 +120,7 @@ void CNNClassifier::outputDebugImage(
   cv::vconcat(debug_image, text_img, debug_image);
 
   sensor_msgs::ImagePtr debug_image_msg =
-    cv_bridge::CvImage(std_msgs::Header(), "bgr8", debug_image).toImageMsg();
+    cv_bridge::CvImage(std_msgs::Header(), "rgb8", debug_image).toImageMsg();
   image_pub_.publish(debug_image_msg);
 }
 
@@ -128,7 +129,7 @@ void CNNClassifier::preProcess(cv::Mat & image, float * input_tensor, bool norma
   /* normalize */
   /* ((channel[0] / 255) - mean[0]) / std[0] */
 
-  cv::cvtColor(image, image, cv::COLOR_BGR2RGB, 3);
+  // cv::cvtColor(image, image, cv::COLOR_BGR2RGB, 3);
   cv::resize(image, image, cv::Size(input_w_, input_h_));
 
   const size_t strides_cv[3] = {static_cast<size_t>(input_w_ * input_c_),
@@ -174,7 +175,7 @@ bool CNNClassifier::postProcess(
   boost::algorithm::split(splited_label, match_label, boost::is_any_of(","));
   for (auto label : splited_label) {
     if (label2state_.find(label) == label2state_.end()) {
-      ROS_DEBUG("cnn_classifier does not have a key [%s]", label);
+      ROS_DEBUG("cnn_classifier does not have a key [%s]", label.c_str());
       continue;
     }
     autoware_perception_msgs::LampState state;

diff --git a/perception/traffic_light_classifier/src/color_classifier.cpp b/perception/traffic_light_classifier/src/color_classifier.cpp
@@ -19,9 +19,10 @@
 
 namespace traffic_light
 {
-ColorClassifier::ColorClassifier()
-: nh_(""), pnh_("~"), image_transport_(pnh_), ratio_threshold_(0.02)
+ColorClassifier::ColorClassifier(const ros::NodeHandle & nh, const ros::NodeHandle & pnh)
+: nh_(nh), pnh_(pnh), image_transport_(pnh), ratio_threshold_(0.02)
 {
+  image_transport_ = image_transport::ImageTransport(pnh_);
   image_pub_ = image_transport_.advertise("debug/image", 1);
   dynamic_reconfigure_.setCallback(boost::bind(&ColorClassifier::parametersCallback, this, _1, _2));
 }

diff --git a/perception/traffic_light_classifier/src/main.cpp b/perception/traffic_light_classifier/src/main.cpp