add d2c viewer

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