diff --git a/README.MD b/README.MD index 0eef390a..9015817c 100644 --- a/README.MD +++ b/README.MD @@ -334,6 +334,8 @@ Launch camera node source ~/ros2_ws/install/setup.bash ros2 run orbbec_camera list_devices_node # Check if the camera is connected ros2 launch orbbec_camera gemini_330_series.launch.py # Or other launch file, see below table +# Optional: enable Jet colorized depth on /camera/depth/image_raw +ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true ``` - On terminal 2 @@ -367,6 +369,10 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}' For more usage details, please refer to the official [OrbbecSDK ROS2 documentation](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/4_application_guide/application_guide.html) +When `enable_depth_colorizer` is enabled, `depth/image_raw` directly publishes an `rgb8` image +(OpenCV Jet). When disabled, the topic continues to publish the raw `16UC1` depth image; the +colorized image uses a 0--10 m depth range for mapping. + ## Supported Devices Currently, the following devices are supported by the OrbbecSDK ROS2 Wrapper v2-main branch. More devices support will be added in the near future. If you can not find your device in the table below, try the [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) branch. diff --git a/README_CN.MD b/README_CN.MD index 0ae10dea..3ace3e42 100644 --- a/README_CN.MD +++ b/README_CN.MD @@ -333,6 +333,8 @@ sudo bash install_udev_rules.sh source ~/ros2_ws/install/setup.bash ros2 run orbbec_camera list_devices_node #检查相机是否已连接 ros2 launch orbbec_camera gemini_330_series.launch.py # 或其他启动文件,见下表 +# 可选:开启 Jet 伪彩色深度图,直接发布到 /camera/depth/image_raw +ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true ``` - 终端 2 @@ -366,6 +368,9 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}' 更多使用详情,请参考官方 [OrbbecSDK ROS2 文档](https://orbbec.github.io/OrbbecSDK_ROS2/zh/source/camera_devices/4_application_guide/application_guide.html) +开启 `enable_depth_colorizer` 后,`depth/image_raw` 直接发布 `rgb8` 彩色图(OpenCV Jet)。 +关闭时该话题仍发布原始 `16UC1` 深度图,彩色图使用 0--10 m 深度范围进行映射。 + ## 支持的设备 目前 v2-main 分支支持以下设备。更多设备支持将陆续增加。如果没有找到您的设备,请尝试 [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) 分支。 diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml index 73587fce..6733d17c 100644 --- a/orbbec_camera/config/camera_params.yaml +++ b/orbbec_camera/config/camera_params.yaml @@ -1,6 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false +enable_depth_colorizer: false enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false @@ -61,5 +62,3 @@ right_ir_format: "ANY" # ir_height: 0 # ir_fps: 0 # ir_format: "ANY" - - diff --git a/orbbec_camera/config/camera_secondary_params.yaml b/orbbec_camera/config/camera_secondary_params.yaml index cf60bae9..eb23535e 100644 --- a/orbbec_camera/config/camera_secondary_params.yaml +++ b/orbbec_camera/config/camera_secondary_params.yaml @@ -1,6 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false +enable_depth_colorizer: false enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false @@ -61,5 +62,3 @@ right_ir_format: "ANY" # ir_height: 0 # ir_fps: 0 # ir_format: "ANY" - - diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index f21ad31c..f40cd81f 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -558,6 +558,8 @@ class OBCameraNode { void publishRawDepthImage(const std::shared_ptr& depth_frame); + cv::Mat colorizeDepthImage(const cv::Mat& depth_image); + std::shared_ptr processDepthFrameFilter(std::shared_ptr& frame); std::shared_ptr processColorFrameFilter(std::shared_ptr& frame); @@ -799,6 +801,7 @@ class OBCameraNode { std::string color_info_url_; std::string ir_info_url_; std::optional camera_param_; + bool colorizer_enabled_ = false; bool enable_d2c_viewer_ = false; std::unique_ptr d2c_viewer_ = nullptr; std::map save_images_; diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 0a498db9..ba4217b6 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -68,6 +68,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ordered_pc', default_value='false'), DeclareLaunchArgument('use_hardware_time', default_value='false'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index d17f738d..f8d55d97 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -100,6 +100,7 @@ def generate_launch_description(): DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'), DeclareLaunchArgument('hole_filling_filter_mode', default_value=''), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("ordered_pc", default_value="false"), DeclareLaunchArgument("enable_depth_scale", default_value="true"), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index 33f2a11c..f7459b5c 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -158,6 +158,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 1773024c..d37939f4 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -158,6 +158,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 95a1eec0..6110f973 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -77,6 +77,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_level', default_value='info'), DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 02757996..16b84b45 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -86,6 +86,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'), DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'), DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 6ea3f7aa..2d4253d1 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -77,6 +77,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("enable_frame_sync", default_value="false"), DeclareLaunchArgument("ordered_pc", default_value="false"), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index 7283f469..6ac85e79 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -80,6 +80,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 040e0696..8b89b86d 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -86,6 +86,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index d0beb879..7b3bd1bc 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -92,6 +92,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index e5da7d5f..10219f69 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -90,6 +90,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 81405065..64d27153 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -146,6 +146,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), + DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), DeclareLaunchArgument("disparity_to_depth_mode", default_value="SW"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("enable_decimation_filter", default_value="false"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index fd1e3167..fac15ca8 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -157,6 +157,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index 87e46fdd..75159fc3 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -158,6 +158,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 726472be..fac6695b 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -188,6 +188,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 4339900b..88c5ed7a 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -206,6 +206,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('sync_mode', default_value='standalone'), DeclareLaunchArgument('depth_delay_us', default_value='0'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index a8860183..2b61316f 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -212,6 +212,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='false'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 36be361e..14364ec0 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -199,6 +199,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='false'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index a41be6b1..e5d04a92 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py @@ -113,6 +113,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_frame_drop_log', default_value='false'), DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('show_fps_enable', default_value='false'), DeclareLaunchArgument('enumerate_net_device', default_value='true'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 21cc4ae2..1149f3b2 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -117,6 +117,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('sync_mode', default_value='standalone'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index b4874abe..94fcd0f7 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -66,6 +66,7 @@ namespace { constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800"; constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16"; +constexpr double kColorizerMaxDistanceMm = 10000.0; std::string getDepthFilterStatusName(const std::string &filter_name) { if (filter_name == "SpatialAdvancedFilter") { @@ -4466,6 +4467,7 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(point_cloud_decimation_filter_factor_, "point_cloud_decimation_filter_factor", 1); setAndGetNodeParameter(point_cloud_qos_, "point_cloud_qos", "default"); + setAndGetNodeParameter(colorizer_enabled_, "enable_depth_colorizer", false); setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false); setAndGetNodeParameter(disparity_to_depth_mode_, "disparity_to_depth_mode", ""); disparity_to_depth_mode_ = @@ -5428,6 +5430,11 @@ void OBCameraNode::setupPublishers() { } } + if (colorizer_enabled_ && enable_stream_[DEPTH]) { + RCLCPP_INFO(logger_, + "Depth colorizer enabled, publishing Jet RGB images on depth/image_raw (0-10 m)"); + } + syncSoftwareAlignment(); if (enable_sync_output_accel_gyro_) { @@ -5601,6 +5608,34 @@ void OBCameraNode::publishRawDepthImage(const std::shared_ptr &depth_ depth_unaligned_publisher_->publish(std::move(image_msg)); } +cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) { + if (depth_image.empty() || depth_image.channels() != 1) { + return {}; + } + + cv::Mat depth_8u; + if (depth_image.type() == CV_16UC1 || depth_image.type() == CV_32FC1) { + depth_image.convertTo(depth_8u, CV_8UC1, 255.0 / kColorizerMaxDistanceMm); + } else if (depth_image.type() == CV_8UC1) { + depth_image.convertTo(depth_8u, CV_8UC1); + } else { + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000, + "Unsupported depth image type for colorizer: %d", depth_image.type()); + return {}; + } + + cv::Mat colorized_bgr; + cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET); + + cv::Mat invalid_depth_mask; + cv::compare(depth_image, cv::Scalar(0), invalid_depth_mask, cv::CMP_EQ); + colorized_bgr.setTo(cv::Scalar::all(0), invalid_depth_mask); + + cv::Mat colorized_rgb; + cv::cvtColor(colorized_bgr, colorized_rgb, cv::COLOR_BGR2RGB); + return colorized_rgb; +} + void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &frame_set) { if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 || !enable_point_cloud_) { @@ -6785,22 +6820,34 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, camera_info_publishers_[stream_index]->publish(camera_info); publishMetadata(frame, stream_index, camera_info.header); - if (!has_raw_image_subscriber && !save_images_[stream_index]) { - return; - } + cv::Mat image_to_publish = image; + std::string image_encoding = encoding_[stream_index]; + uint32_t image_step = width * unit_step_size_[stream_index]; if (stream_index == DEPTH) { auto depth_scale = video_frame->as()->getValueScale(); image = image * depth_scale; + image_to_publish = image; + if (colorizer_enabled_ && has_raw_image_subscriber) { + auto colorized_image = colorizeDepthImage(image); + if (!colorized_image.empty()) { + image_to_publish = std::move(colorized_image); + image_encoding = sensor_msgs::image_encodings::RGB8; + image_step = static_cast(image_to_publish.cols * 3 * sizeof(uint8_t)); + } + } + } + if (!has_raw_image_subscriber && !save_images_[stream_index]) { + return; } CHECK(image_publishers_.count(stream_index) > 0); if (has_raw_image_subscriber || save_images_[stream_index]) { sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image()); - cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image) + cv_bridge::CvImage(std_msgs::msg::Header(), image_encoding, image_to_publish) .toImageMsg(*image_msg); CHECK_NOTNULL(image_msg.get()); image_msg->header.stamp = timestamp; image_msg->is_bigendian = false; - image_msg->step = width * unit_step_size_[stream_index]; + image_msg->step = image_step; image_msg->header.frame_id = frame_id; saveImageToFile(stream_index, image, *image_msg); if (!has_raw_image_subscriber) {