fix(image_projection_based_fusion): add run length decoding for segme…

…ntation_pointcloud_fusion (autowarefoundation#7909)

* fix: add rle decompress

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* style(pre-commit): autofix

* fix: use rld in tensorrt utils

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: rebase error

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: dependency

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: skip publish debug mask

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* Update perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

* style(pre-commit): autofix

* Revert "fix: skip publish debug mask"

This reverts commit 30fa9ae.

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

@@ -22,3 +22,6 @@
 
     # the maximum distance of pointcloud to be applied filter
     filter_distance_threshold: 60.0
+
+    # Avoid using debug mask in case of multiple camera semantic segmentation fusion
+    is_publish_debug_mask: false

perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

@@ -16,6 +16,7 @@
 #define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
 
 #include "image_projection_based_fusion/fusion_node.hpp"
+#include <image_transport/image_transport.hpp>
 
 #include <string>
 #include <utility>
@@ -42,6 +43,9 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
     {"BIKE", false},          {"ROAD", false},         {"SIDEWALK", false},   {"ROAD_PAINT", false},
     {"CURBSTONE", false},     {"CROSSWALK", false},    {"VEGETATION", false}, {"SKY", false}};
 
+  image_transport::Publisher pub_debug_mask_ptr_;
+  bool is_publish_debug_mask_;
+
 public:
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
 

perception/image_projection_based_fusion/package.xml

@@ -27,6 +27,7 @@
   <depend>object_recognition_utils</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
+  <depend>perception_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>

perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json

@@ -114,9 +114,18 @@
           "description": "A maximum distance of pointcloud to apply filter [m].",
           "default": 60.0,
           "minimum": 0.0
+        },
+        "is_publish_debug_mask": {
+          "type": "boolean",
+          "description": "If true, debug mask image will be published.",
+          "default": false
         }
       },
-      "required": ["filter_semantic_label_target", "filter_distance_threshold"]
+      "required": [
+        "filter_semantic_label_target",
+        "filter_distance_threshold",
+        "is_publish_debug_mask"
+      ]
     }
   },
   "properties": {

perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -17,6 +17,8 @@
 #include "image_projection_based_fusion/utils/geometry.hpp"
 #include "image_projection_based_fusion/utils/utils.hpp"
 
+#include <perception_utils/run_length_encoder.hpp>
+
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
@@ -38,6 +40,8 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio
     RCLCPP_INFO(
       this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second);
   }
+  is_publish_debug_mask_ = declare_parameter<bool>("is_publish_debug_mask");
+  pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask");
 }
 
 void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
@@ -57,18 +61,18 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
-  cv_bridge::CvImagePtr in_image_ptr;
-  try {
-    in_image_ptr = cv_bridge::toCvCopy(
-      std::make_shared<sensor_msgs::msg::Image>(input_mask), input_mask.encoding);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what());
+  if (input_mask.height == 0 || input_mask.width == 0) {
     return;
   }
+  std::vector<uint8_t> mask_data(input_mask.data.begin(), input_mask.data.end());
+  cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width);
 
-  cv::Mat mask = in_image_ptr->image;
-  if (mask.cols == 0 || mask.rows == 0) {
-    return;
+  // publish debug mask
+  if (is_publish_debug_mask_) {
+    sensor_msgs::msg::Image::SharedPtr debug_mask_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", mask).toImageMsg();
+    debug_mask_msg->header = input_mask.header;
+    pub_debug_mask_ptr_.publish(debug_mask_msg);
   }
   const int orig_width = camera_info.width;
   const int orig_height = camera_info.height;