fix(image_projection_based_fusion): revise message publishers (#9865)

* refactor: fix condition for publishing painted pointcloud message

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: publish output revised

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: fix condition for publishing painted pointcloud message

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: roi-pointclout  fusion - publish empty image even when there is no target roi

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: remap output topic for clusters in roi_pointcloud_fusion

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: fix condition for publishing painted pointcloud message

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: Add debug publisher for internal debugging

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: remove !! pointer to bool conversion

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: technolojin

launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml

@@ -181,7 +181,7 @@
           <arg name="input/image6" value="$(var input/camera6/image)"/>
           <arg name="input/image7" value="$(var input/camera7/image)"/>
           <arg name="input/pointcloud" value="$(var roi_cluster/input/pointcloud)"/>
-          <arg name="output_clusters" value="$(var roi_cluster/output/clusters)"/>
+          <arg name="output/clusters" value="$(var roi_cluster/output/clusters)"/>
           <arg name="param_path" value="$(var roi_pointcloud_fusion_param_path)"/>
         </include>
       </group>

perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md

@@ -16,18 +16,18 @@ The lidar points are projected onto the output of an image-only 2d object detect
 
 | Name                     | Type                                                     | Description                                               |
 | ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- |
-| `input`                  | `sensor_msgs::msg::PointCloud2`                          | pointcloud                                                |
+| `input/pointcloud`       | `sensor_msgs::msg::PointCloud2`                          | pointcloud                                                |
 | `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo`                           | camera information to project 3d points onto image planes |
 | `input/rois[0-7]`        | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image                                      |
 | `input/image_raw[0-7]`   | `sensor_msgs::msg::Image`                                | images for visualization                                  |
 
 ### Output
 
-| Name                     | Type                                             | Description              |
-| ------------------------ | ------------------------------------------------ | ------------------------ |
-| `output`                 | `sensor_msgs::msg::PointCloud2`                  | painted pointcloud       |
-| `~/output/objects`       | `autoware_perception_msgs::msg::DetectedObjects` | detected objects         |
-| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image`                        | images for visualization |
+| Name                       | Type                                             | Description              |
+| -------------------------- | ------------------------------------------------ | ------------------------ |
+| `output/objects`           | `autoware_perception_msgs::msg::DetectedObjects` | detected objects         |
+| `debug/painted_pointcloud` | `sensor_msgs::msg::PointCloud2`                  | painted pointcloud       |
+| `debug/image_raw[0-7]`     | `sensor_msgs::msg::Image`                        | images for visualization |
 
 ## Parameters
 

perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md

@@ -23,10 +23,10 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a
 
 ### Output
 
-| Name                     | Type                                                     | Description                |
-| ------------------------ | -------------------------------------------------------- | -------------------------- |
-| `output`                 | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud |
-| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image`                                | images for visualization   |
+| Name                   | Type                                                     | Description                |
+| ---------------------- | -------------------------------------------------------- | -------------------------- |
+| `output`               | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud |
+| `debug/image_raw[0-7]` | `sensor_msgs::msg::Image`                                | images for visualization   |
 
 ## Parameters
 

perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md

@@ -30,12 +30,12 @@ The DetectedObject has three possible shape choices/implementations, where the p
 
 ### Output
 
-| Name                      | Type                                             | Description                |
-| ------------------------- | ------------------------------------------------ | -------------------------- |
-| `output`                  | `autoware_perception_msgs::msg::DetectedObjects` | detected objects           |
-| `~/debug/image_raw[0-7]`  | `sensor_msgs::msg::Image`                        | images for visualization,  |
-| `~/debug/fused_objects`   | `autoware_perception_msgs::msg::DetectedObjects` | fused detected objects     |
-| `~/debug/ignored_objects` | `autoware_perception_msgs::msg::DetectedObjects` | not fused detected objects |
+| Name                    | Type                                             | Description                |
+| ----------------------- | ------------------------------------------------ | -------------------------- |
+| `output`                | `autoware_perception_msgs::msg::DetectedObjects` | detected objects           |
+| `debug/image_raw[0-7]`  | `sensor_msgs::msg::Image`                        | images for visualization,  |
+| `debug/fused_objects`   | `autoware_perception_msgs::msg::DetectedObjects` | fused detected objects     |
+| `debug/ignored_objects` | `autoware_perception_msgs::msg::DetectedObjects` | not fused detected objects |
 
 ## Parameters
 

perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md

@@ -24,11 +24,10 @@ The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of
 
 ### Output
 
-| Name              | Type                                                     | Description                                  |
-| ----------------- | -------------------------------------------------------- | -------------------------------------------- |
-| `output`          | `sensor_msgs::msg::PointCloud2`                          | output pointcloud as default of interface    |
-| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters                              |
-| `debug/clusters`  | `sensor_msgs/msg/PointCloud2`                            | colored cluster pointcloud for visualization |
+| Name             | Type                                                     | Description                                  |
+| ---------------- | -------------------------------------------------------- | -------------------------------------------- |
+| `output`         | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters                              |
+| `debug/clusters` | `sensor_msgs/msg/PointCloud2`                            | colored cluster pointcloud for visualization |
 
 ## Parameters
 

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

@@ -150,8 +150,8 @@ class FusionNode : public rclcpp::Node
   // 2d detection management
   std::vector<Det2dStatus<Msg2D>> det2d_list_;
 
-  /** \brief A vector of subscriber. */
-  typename rclcpp::Subscription<Msg3D>::SharedPtr sub_;
+  // 3d detection subscription
+  typename rclcpp::Subscription<Msg3D>::SharedPtr det3d_sub_;
 
   // parameters for out_of_scope filter
   float filter_scope_min_x_;
@@ -166,6 +166,7 @@ class FusionNode : public rclcpp::Node
 
   // debugger
   std::shared_ptr<Debugger> debugger_;
+  std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_internal_pub_;
 
   /** \brief processing time publisher. **/
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -57,7 +57,7 @@ class PointPaintingFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType,
   void postprocess(
     const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override;
 
-  rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
+  rclcpp::Publisher<PointCloudMsgType>::SharedPtr painted_point_pub_ptr_;
   std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   int omp_num_threads_{1};

perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml

@@ -17,8 +17,7 @@
   <arg name="input/rois7" default="rois7"/>
   <arg name="input/camera_info7" default="/camera_info7"/>
   <arg name="input/pointcloud" default="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
-  <arg name="output/pointcloud" default="roi_pointcloud_fusion/output/pointcloud"/>
-  <arg name="output_clusters" default="output_clusters"/>
+  <arg name="output/clusters" default="output/clusters"/>
   <arg name="debug/clusters" default="roi_pointcloud_fusion/debug/clusters"/>
   <arg name="param_path" default="$(find-pkg-share autoware_image_projection_based_fusion)/config/roi_pointcloud_fusion.param.yaml"/>
   <arg name="sync_param_path" default="$(find-pkg-share autoware_image_projection_based_fusion)/config/fusion_common.param.yaml"/>
@@ -39,8 +38,7 @@
       <param from="$(var param_path)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/pointcloud)"/>
-      <remap from="output" to="$(var output/pointcloud)"/>
-      <remap from="output_clusters" to="$(var output_clusters)"/>
+      <remap from="output" to="$(var output/clusters)"/>
       <remap from="debug/clusters" to="$(var debug/clusters)"/>
 
       <!-- rois, camera and info -->

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -105,7 +105,8 @@ FusionNode<Msg3D, Msg2D, ExportObj>::FusionNode(
   // subscribe 3d detection
   std::function<void(const typename Msg3D::ConstSharedPtr msg)> sub_callback =
     std::bind(&FusionNode::subCallback, this, std::placeholders::_1);
-  sub_ = this->create_subscription<Msg3D>("input", rclcpp::QoS(1).best_effort(), sub_callback);
+  det3d_sub_ =
+    this->create_subscription<Msg3D>("input", rclcpp::QoS(1).best_effort(), sub_callback);
 
   // Set timer
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -141,6 +142,10 @@ FusionNode<Msg3D, Msg2D, ExportObj>::FusionNode(
       static_cast<std::size_t>(declare_parameter<int32_t>("image_buffer_size"));
     debugger_ =
       std::make_shared<Debugger>(this, rois_number, image_buffer_size, input_camera_topics);
+
+    // input topic timing publisher
+    debug_internal_pub_ =
+      std::make_unique<autoware::universe_utils::DebugPublisher>(this, get_name());
   }
 
   // time keeper
@@ -155,10 +160,9 @@ FusionNode<Msg3D, Msg2D, ExportObj>::FusionNode(
 
   // initialize debug tool
   {
-    using autoware::universe_utils::DebugPublisher;
-    using autoware::universe_utils::StopWatch;
-    stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
-    debug_publisher_ = std::make_unique<DebugPublisher>(this, get_name());
+    stop_watch_ptr_ =
+      std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ = std::make_unique<autoware::universe_utils::DebugPublisher>(this, get_name());
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
@@ -347,11 +351,11 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
       det2d.is_fused = true;
 
       // add timestamp interval for debug
-      if (debug_publisher_) {
+      if (debug_internal_pub_) {
         double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6;
-        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+        debug_internal_pub_->publish<tier4_debug_msgs::msg::Float64Stamped>(
           "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms);
-        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+        debug_internal_pub_->publish<tier4_debug_msgs::msg::Float64Stamped>(
           "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms",
           timestamp_interval_ms - det2d.input_offset_ms);
       }
@@ -412,11 +416,11 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::roiCallback(
       fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_));
       det2d.is_fused = true;
 
-      if (debug_publisher_) {
+      if (debug_internal_pub_) {
         double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6;
-        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+        debug_internal_pub_->publish<tier4_debug_msgs::msg::Float64Stamped>(
           "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms);
-        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+        debug_internal_pub_->publish<tier4_debug_msgs::msg::Float64Stamped>(
           "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms",
           timestamp_interval_ms - det2d.input_offset_ms);
       }

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -158,12 +158,13 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   // subscriber
   std::function<void(const PointCloudMsgType::ConstSharedPtr msg)> sub_callback =
     std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1);
-  sub_ = this->create_subscription<PointCloudMsgType>(
+  det3d_sub_ = this->create_subscription<PointCloudMsgType>(
     "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback);
 
   // publisher
-  point_pub_ptr_ = this->create_publisher<PointCloudMsgType>("output", rclcpp::QoS{1});
   pub_ptr_ = this->create_publisher<DetectedObjects>("~/output/objects", rclcpp::QoS{1});
+  painted_point_pub_ptr_ =
+    this->create_publisher<PointCloudMsgType>("~/debug/painted_pointcloud", rclcpp::QoS{1});
 
   detection_class_remapper_.setParameters(
     allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix);
@@ -438,8 +439,8 @@ void PointPaintingFusionNode::postprocess(
   detection_class_remapper_.mapClasses(output_msg);
 
   // publish debug message: painted pointcloud
-  if (point_pub_ptr_->get_subscription_count() > 0) {
-    point_pub_ptr_->publish(painted_pointcloud_msg);
+  if (debugger_ && painted_point_pub_ptr_->get_subscription_count() > 0) {
+    painted_point_pub_ptr_->publish(painted_pointcloud_msg);
   }
   diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp);
 }

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -334,8 +334,11 @@ void RoiDetectedObjectFusionNode::postprocess(
       debug_ignored_objects_msg.objects.emplace_back(obj);
     }
   }
-  debug_publisher_->publish<DetectedObjects>("debug/fused_objects", debug_fused_objects_msg);
-  debug_publisher_->publish<DetectedObjects>("debug/ignored_objects", debug_ignored_objects_msg);
+  if (debug_internal_pub_) {
+    debug_internal_pub_->publish<DetectedObjects>("debug/fused_objects", debug_fused_objects_msg);
+    debug_internal_pub_->publish<DetectedObjects>(
+      "debug/ignored_objects", debug_ignored_objects_msg);
+  }
 
   // clear flags
   passthrough_object_flags_map_.erase(timestamp_nsec);

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -45,8 +45,7 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt
   cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance");
 
   // publisher
-  point_pub_ptr_ = this->create_publisher<PointCloudMsgType>("output", rclcpp::QoS{1});
-  pub_ptr_ = this->create_publisher<ClusterMsgType>("output_clusters", rclcpp::QoS{1});
+  pub_ptr_ = this->create_publisher<ClusterMsgType>("output", rclcpp::QoS{1});
   cluster_debug_pub_ = this->create_publisher<PointCloudMsgType>("debug/clusters", 1);
 }
 
@@ -80,7 +79,15 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
       debug_image_rois.push_back(feature_obj.feature.roi);
     }
   }
+
+  // check if there is no object to fuse
   if (output_objs.empty()) {
+    // publish debug image, which is empty
+    if (debugger_) {
+      debugger_->image_rois_ = debug_image_rois;
+      debugger_->obstacle_points_ = debug_image_points;
+      debugger_->publishImage(det2d.id, input_roi_msg.header.stamp);
+    }
     return;
   }
 
@@ -164,6 +171,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   updateOutputFusedObjects(
     output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header,
     tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_);
+
+  // publish debug image
   if (debugger_) {
     debugger_->image_rois_ = debug_image_rois;
     debugger_->obstacle_points_ = debug_image_points;
@@ -188,9 +197,6 @@ void RoiPointCloudFusionNode::postprocess(
     autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg);
     cluster_debug_pub_->publish(debug_cluster_msg);
   }
-  if (point_pub_ptr_->get_subscription_count() > 0) {
-    point_pub_ptr_->publish(pointcloud_msg);
-  }
 }
 
 void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg)