Add cloud accumulated

orbbec_camera/src/cloud_accumulated.cpp

@@ -0,0 +1,78 @@
+/*******************************************************************************
+ * Copyright (c) 2023 Orbbec 3D Technology, Inc
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *******************************************************************************/
+
+#if defined(ROS_JAZZY) || defined(ROS_IRON)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+#include "orbbec_camera/cloud_accumulated.h"
+
+namespace orbbec_camera {
+namespace orbbec_lidar {
+CloudAccumulated::CloudAccumulated(rclcpp::Node* const node, rmw_qos_profile_t cloud_qos,
+                                   int cloud_accumulation_count): node_(node), logger_(rclcpp::get_logger("cloud_accumulated")) {
+                                    cloud_sub_=this->create_subscription<sensor_msgs::msg::PointCloud2>(
+            "/points", cloud_qos, std::bind(&CloudAccumulated::cloud_callback, this, _1));
+                                   }
+}  // namespace orbbec_lidar
+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_encode = (rgb_msg->step == 5760) ? sensor_msgs::image_encodings::RGB8
+                                            : sensor_msgs::image_encodings::RGBA8;
+  auto gray_type = (rgb_msg->step == 5760) ? cv::COLOR_GRAY2RGB : cv::COLOR_GRAY2RGBA;
+  auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, rgb_encode);
+  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, gray_type);
+  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(), rgb_encode, d2c_img).toImageMsg();
+  if (d2c_msg != nullptr) {
+    d2c_msg->header = rgb_msg->header;
+    d2c_viewer_pub_->publish(*d2c_msg);
+  } else {
+    RCLCPP_ERROR(logger_,
+                 "-----------------------d2c_viewer publishing failed-----------------------");
+  }
+}
+}  // namespace orbbec_camera

orbbec_camera/src/ob_lidar_node.cpp

@@ -53,6 +53,10 @@ OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
   jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #endif
   is_camera_node_initialized_ = true;
+  if (enable_cloud_accumulated_&&cloud_accumulation_count_>=1) {
+    auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
+    cloud_accumulated_=std::make_unique<CloudAccumulated>(node_, point_cloud_qos_profile, cloud_accumulation_count_);
+  }
 }
 
 template <class T>
@@ -148,6 +152,8 @@ void OBLidarNode::getParameters() {
   setAndGetNodeParameter<int>(repetitive_scan_mode_, "repetitive_scan_mode", -1);
   setAndGetNodeParameter<int>(filter_level_, "filter_level", -1);
   setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
+  setAndGetNodeParameter<bool>(enable_cloud_accumulated_, "enable_cloud_accumulated", false);
+  setAndGetNodeParameter<bool>(cloud_accumulation_count_, "cloud_accumulation_count", -1);
 }
 
 void OBLidarNode::setupDevices() {