feat: add support for colorized depth images in camera node and update launch configurations

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.

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) 分支。

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"
-
-

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"
-
-

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -558,6 +558,8 @@ class OBCameraNode {
 
   void publishRawDepthImage(const std::shared_ptr<ob::Frame>& depth_frame);
 
+  cv::Mat colorizeDepthImage(const cv::Mat& depth_image);
+
   std::shared_ptr<ob::Frame> processDepthFrameFilter(std::shared_ptr<ob::Frame>& frame);
 
   std::shared_ptr<ob::Frame> processColorFrameFilter(std::shared_ptr<ob::Frame>& frame);
@@ -799,6 +801,7 @@ class OBCameraNode {
   std::string color_info_url_;
   std::string ir_info_url_;
   std::optional<OBCameraParam> camera_param_;
+  bool colorizer_enabled_ = false;
   bool enable_d2c_viewer_ = false;
   std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
   std::map<stream_index_pair, std::atomic_bool> save_images_;

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'),

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"),

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'),

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'),

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'),

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'),

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"),

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'),

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'),

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'),

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'),

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"),

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'),

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'),

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'),

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'),

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'),

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'),

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'),

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'),

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<int>(point_cloud_decimation_filter_factor_,
                               "point_cloud_decimation_filter_factor", 1);
   setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
+  setAndGetNodeParameter<bool>(colorizer_enabled_, "enable_depth_colorizer", false);
   setAndGetNodeParameter<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
   setAndGetNodeParameter<std::string>(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<ob::Frame> &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<ob::FrameSet> &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<ob::Frame> &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<ob::DepthFrame>()->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<uint32_t>(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) {