From 4d1ce1cb91a416b1965309690c90f3bccdac523d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 8 Jul 2024 22:04:11 +0900 Subject: [PATCH 1/9] fix: add rle decompress Signed-off-by: badai-nguyen --- .../segmentation_pointcloud_fusion/node.hpp | 2 + .../segmentation_pointcloud_fusion/node.cpp | 39 ++++++++++++++----- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index c57d455300125..fd36d471395e7 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -48,6 +48,8 @@ class SegmentPointCloudFusionNode : public FusionNode & rle_data, const int rows, const int cols); void preprocess(PointCloud2 & pointcloud_msg) override; void postprocess(PointCloud2 & pointcloud_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 5c93fa6c2fd1b..97b57391a94e1 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -49,6 +49,32 @@ void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud { return; } +cv::Mat SegmentPointCloudFusionNode::rle_decompress( + const std::vector & rle_data, const int rows, const int cols) +{ + cv::Mat mask(rows, cols, CV_8UC1, cv::Scalar(0)); + int idx = 0; + int step = sizeof(uint8_t) + sizeof(int); + int nb_pixels = 0; + for (size_t i = 0; i < rle_data.size(); i += step) { + uint8_t value; + int length; + std::memcpy(&value, &rle_data[i], sizeof(uint8_t)); + std::memcpy(&length, &rle_data[i + sizeof(uint8_t)], sizeof(int)); + nb_pixels += length; + for (int j = 0; j < length; ++j) { + int row_idx = static_cast(idx / cols); + int col_idx = static_cast(idx % cols); + mask.at(row_idx, col_idx) = value; + idx++; + if (idx > rows * cols) { + RCLCPP_ERROR(this->get_logger(), "RLE decompression error: idx out of bound"); + break; + } + } + } + return mask; +} void SegmentPointCloudFusionNode::fuseOnSingleImage( const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, @@ -57,19 +83,12 @@ 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(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; } + cv::Mat mask = SegmentPointCloudFusionNode::rle_decompress( + input_mask.data, input_mask.height, input_mask.width); - cv::Mat mask = in_image_ptr->image; - if (mask.cols == 0 || mask.rows == 0) { - return; - } const int orig_width = camera_info.width; const int orig_height = camera_info.height; // resize mask to the same size as the camera image From 12f0b2b3f63db9f77d4b22b2a7b9112b093ba1eb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 9 Jul 2024 05:00:36 +0000 Subject: [PATCH 2/9] style(pre-commit): autofix --- .../segmentation_pointcloud_fusion/node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index fd36d471395e7..25a86ab54a500 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -48,7 +48,6 @@ class SegmentPointCloudFusionNode : public FusionNode & rle_data, const int rows, const int cols); void preprocess(PointCloud2 & pointcloud_msg) override; From 9ea3cdffc21c82341f351a542eda921cc07e4ef4 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 26 Jul 2024 16:25:24 +0900 Subject: [PATCH 3/9] fix: use rld in tensorrt utils Signed-off-by: badai-nguyen --- .../segmentation_pointcloud_fusion.param.yaml | 1 + .../segmentation_pointcloud_fusion/node.hpp | 5 ++- .../package.xml | 1 + ...segmentation_pointcloud_fusion.schema.json | 11 ++++- .../segmentation_pointcloud_fusion/node.cpp | 44 +++++++------------ 5 files changed, 32 insertions(+), 30 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index fdabb0c7055d8..444f7d92ede4e 100644 --- a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -22,3 +22,4 @@ # the maximum distance of pointcloud to be applied filter filter_distance_threshold: 60.0 + is_publish_debug_mask: false diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 25a86ab54a500..af7a4cb18ad61 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -18,6 +18,7 @@ #include "autoware/image_projection_based_fusion/fusion_node.hpp" #include +#include #include #include @@ -44,11 +45,13 @@ class SegmentPointCloudFusionNode : public FusionNode & rle_data, const int rows, const int cols); void preprocess(PointCloud2 & pointcloud_msg) override; void postprocess(PointCloud2 & pointcloud_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index 74ab1b3454899..40f8a13f3ca89 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -20,6 +20,7 @@ autoware_lidar_centerpoint autoware_perception_msgs autoware_point_types + autoware_tensorrt_yolox autoware_universe_utils cv_bridge image_geometry diff --git a/perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json index f5ab1be5eac34..a21ad583af468 100644 --- a/perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json +++ b/perception/autoware_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": { diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 97b57391a94e1..a3b87b66e5481 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -16,6 +16,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include "autoware/tensorrt_yolox/utils.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -38,6 +39,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("is_publish_debug_mask"); + pub_debug_mask_ptr_ = image_transport::create_publisher(this, "debug/mask"); } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -49,32 +52,6 @@ void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud { return; } -cv::Mat SegmentPointCloudFusionNode::rle_decompress( - const std::vector & rle_data, const int rows, const int cols) -{ - cv::Mat mask(rows, cols, CV_8UC1, cv::Scalar(0)); - int idx = 0; - int step = sizeof(uint8_t) + sizeof(int); - int nb_pixels = 0; - for (size_t i = 0; i < rle_data.size(); i += step) { - uint8_t value; - int length; - std::memcpy(&value, &rle_data[i], sizeof(uint8_t)); - std::memcpy(&length, &rle_data[i + sizeof(uint8_t)], sizeof(int)); - nb_pixels += length; - for (int j = 0; j < length; ++j) { - int row_idx = static_cast(idx / cols); - int col_idx = static_cast(idx % cols); - mask.at(row_idx, col_idx) = value; - idx++; - if (idx > rows * cols) { - RCLCPP_ERROR(this->get_logger(), "RLE decompression error: idx out of bound"); - break; - } - } - } - return mask; -} void SegmentPointCloudFusionNode::fuseOnSingleImage( const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, @@ -86,9 +63,20 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( if (input_mask.height == 0 || input_mask.width == 0) { return; } - cv::Mat mask = SegmentPointCloudFusionNode::rle_decompress( + cv::Mat mask = autoware::tensorrt_yolox::runLengthDecoder( input_mask.data, input_mask.height, input_mask.width); - + // 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); + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; const int orig_width = camera_info.width; const int orig_height = camera_info.height; // resize mask to the same size as the camera image From b8abe57a82d8655e33bcb24e7ba2e9e2a69ef7fc Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 9 Aug 2024 09:46:24 +0900 Subject: [PATCH 4/9] fix: rebase error Signed-off-by: badai-nguyen --- .../src/segmentation_pointcloud_fusion/node.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index a3b87b66e5481..1566453a5b34b 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -72,11 +72,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( debug_mask_msg->header = input_mask.header; pub_debug_mask_ptr_.publish(debug_mask_msg); } - Eigen::Matrix4d projection; - projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, - 0.0, 1.0; const int orig_width = camera_info.width; const int orig_height = camera_info.height; // resize mask to the same size as the camera image From a812eb274ea2ca5d6d82144b53a911e0253d1223 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 16 Aug 2024 12:30:05 +0900 Subject: [PATCH 5/9] fix: dependency Signed-off-by: badai-nguyen --- .../autoware_image_projection_based_fusion/package.xml | 2 +- .../src/segmentation_pointcloud_fusion/node.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index 40f8a13f3ca89..f08ab04bc6616 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -20,13 +20,13 @@ autoware_lidar_centerpoint autoware_perception_msgs autoware_point_types - autoware_tensorrt_yolox autoware_universe_utils cv_bridge image_geometry image_transport message_filters object_recognition_utils + perception_utils rclcpp rclcpp_components sensor_msgs diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 1566453a5b34b..ca9eba5412449 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -16,7 +16,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" -#include "autoware/tensorrt_yolox/utils.hpp" +#include "perception_utils/run_length_encoder.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -63,8 +63,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( if (input_mask.height == 0 || input_mask.width == 0) { return; } - cv::Mat mask = autoware::tensorrt_yolox::runLengthDecoder( - input_mask.data, input_mask.height, input_mask.width); + std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); + cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); + // publish debug mask if (is_publish_debug_mask_) { sensor_msgs::msg::Image::SharedPtr debug_mask_msg = From 30fa9aed866a019705abde71e8f5c3f98960c19e Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 19 Aug 2024 15:01:23 +0900 Subject: [PATCH 6/9] fix: skip publish debug mask Signed-off-by: badai-nguyen --- .../config/segmentation_pointcloud_fusion.param.yaml | 1 - .../segmentation_pointcloud_fusion/node.hpp | 4 ---- .../schema/segmentation_pointcloud_fusion.schema.json | 11 +---------- .../src/segmentation_pointcloud_fusion/node.cpp | 9 --------- 4 files changed, 1 insertion(+), 24 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 444f7d92ede4e..fdabb0c7055d8 100644 --- a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -22,4 +22,3 @@ # the maximum distance of pointcloud to be applied filter filter_distance_threshold: 60.0 - is_publish_debug_mask: false diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index af7a4cb18ad61..c57d455300125 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -18,7 +18,6 @@ #include "autoware/image_projection_based_fusion/fusion_node.hpp" #include -#include #include #include @@ -45,9 +44,6 @@ class SegmentPointCloudFusionNode : public FusionNodeget_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); } - is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); - pub_debug_mask_ptr_ = image_transport::create_publisher(this, "debug/mask"); } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -67,13 +65,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); - // 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; // resize mask to the same size as the camera image From 75f0eb0ef289b7f34ffbf6a967c6d63295bd8a48 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 20 Aug 2024 15:07:08 +0900 Subject: [PATCH 7/9] Update perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../src/segmentation_pointcloud_fusion/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 9d8c462dd4ecb..789667222d9ea 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -16,7 +16,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" -#include "perception_utils/run_length_encoder.hpp" +#include #ifdef ROS_DISTRO_GALACTIC #include From fd6ce2ef5a8f3e0453660a8796ad413694c02455 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 20 Aug 2024 06:09:53 +0000 Subject: [PATCH 8/9] style(pre-commit): autofix --- .../src/segmentation_pointcloud_fusion/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 789667222d9ea..91f349e1e25f3 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -16,6 +16,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" + #include #ifdef ROS_DISTRO_GALACTIC From cc8e1b8caf90f4eb5b4f973624de68901c96e0b1 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 20 Aug 2024 15:16:04 +0900 Subject: [PATCH 9/9] Revert "fix: skip publish debug mask" This reverts commit 30fa9aed866a019705abde71e8f5c3f98960c19e. --- .../config/segmentation_pointcloud_fusion.param.yaml | 3 +++ .../segmentation_pointcloud_fusion/node.hpp | 4 ++++ .../schema/segmentation_pointcloud_fusion.schema.json | 11 ++++++++++- .../src/segmentation_pointcloud_fusion/node.cpp | 9 +++++++++ 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index fdabb0c7055d8..418a3190d25d4 100644 --- a/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/autoware_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 diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index c57d455300125..af7a4cb18ad61 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -18,6 +18,7 @@ #include "autoware/image_projection_based_fusion/fusion_node.hpp" #include +#include #include #include @@ -44,6 +45,9 @@ class SegmentPointCloudFusionNode : public FusionNodeget_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); } + is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); + pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -66,6 +68,13 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); + // 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; // resize mask to the same size as the camera image