add d2c viewer

orbbec_camera/CMakeLists.txt

@@ -59,6 +59,7 @@ set(ORBBEC_LIBS ${CMAKE_CURRENT_SOURCE_DIR}/SDK/lib/${HOST_PLATFORM})
 set(ORBBEC_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/include/)
 
 add_library(${PROJECT_NAME} SHARED
+  src/d2c_viewer.cpp
   src/dynamic_params.cpp
   src/ob_camera_node_driver.cpp
   src/ob_camera_node.cpp

orbbec_camera/include/orbbec_camera/d2c_viewer.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/time_synchronizer.h>
+#include <sensor_msgs/msg/image.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "utils.h"
+
+namespace orbbec_camera {
+class D2CViewer {
+ public:
+  explicit D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
+                     rmw_qos_profile_t depth_qos);
+  ~D2CViewer();
+
+  void messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg,
+                       const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg);
+
+ private:
+  rclcpp::Node* node_;
+  rclcpp::Logger logger_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> rgb_sub_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_sub_;
+  using MySyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,
+                                                                       sensor_msgs::msg::Image>;
+  std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> sync_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr d2c_viewer_pub_;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -48,6 +48,7 @@
 
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
+#include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
 
 #define STREAM_NAME(sip)                                                                       \
@@ -315,5 +316,7 @@ class OBCameraNode {
   std::string color_info_url_;
   std::string ir_info_url_;
   std::optional<OBCameraParam> camera_param_;
+  bool enable_d2c_viewer_ = false;
+  std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
 };
 }  // namespace orbbec_camera

orbbec_camera/src/d2c_viewer.cpp

@@ -0,0 +1,48 @@
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <opencv2/opencv.hpp>
+
+#include "orbbec_camera/d2c_viewer.h"
+
+namespace orbbec_camera {
+D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
+                     rmw_qos_profile_t depth_qos)
+    : node_(node), logger_(rclcpp::get_logger("d2c_viewer")) {
+  rgb_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
+      node_, "color/image_raw", rgb_qos);
+  depth_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
+      node_, "depth/image_raw", depth_qos);
+  sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(MySyncPolicy(10), *rgb_sub_,
+                                                                        *depth_sub_);
+  sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(1.0));  // 1s
+
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  sync_->registerCallback(std::bind(&D2CViewer::messageCallback, this, _1, _2));
+  d2c_viewer_pub_ =
+      node_->create_publisher<sensor_msgs::msg::Image>("depth_to_color/image_raw", rclcpp::QoS(1));
+}
+D2CViewer::~D2CViewer() = default;
+
+void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
+                                const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) {
+  if (rgb_msg->width != depth_msg->width || rgb_msg->height != depth_msg->height) {
+    RCLCPP_ERROR(logger_, "rgb and depth image size not match(%d, %d) vs (%d, %d)", rgb_msg->width,
+                 rgb_msg->height, depth_msg->width, depth_msg->height);
+    return;
+  }
+  auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::RGB8);
+  auto depth_img_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);
+  cv::Mat gray_depth, depth_img, d2c_img;
+  depth_img_ptr->image.convertTo(gray_depth, CV_8UC1);
+  cv::cvtColor(gray_depth, depth_img, cv::COLOR_GRAY2RGB);
+  depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
+  cv::bitwise_or(rgb_img_ptr->image, depth_img, d2c_img);
+  sensor_msgs::msg::Image::SharedPtr d2c_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::RGB8, d2c_img)
+          .toImageMsg();
+  d2c_msg->header = rgb_msg->header;
+  d2c_viewer_pub_->publish(*d2c_msg);
+}
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -37,6 +37,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
   setupDefaultImageFormat();
   setupTopics();
   startStreams();
+  if (enable_d2c_viewer_) {
+    auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
+    auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]);
+    d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
+  }
 }
 
 template <class T>
@@ -252,6 +257,7 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter(enable_point_cloud_, "enable_point_cloud", true);
   setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
   setAndGetNodeParameter(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
+  setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
   if (enable_colored_point_cloud_) {
     depth_registration_ = true;
   }

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -222,6 +222,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
   int ret = sem_wait(device_sem);
   if (ret != 0) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to wait semaphore " << strerror(errno));
+    releaseDeviceSemaphore(device_sem, num_devices_connected_);
     return nullptr;
   }
   auto device = selectDeviceBySerialNumber(list, serial_number_);