add save point cloud and image

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -30,6 +30,8 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <std_srvs/srv/set_bool.hpp>
+#include <std_srvs/srv/empty.hpp>
+
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>
 
@@ -216,6 +218,12 @@ class OBCameraNode {
 
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
+  void saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
+                         std::shared_ptr<std_srvs::srv::Empty::Response>& response);
+
+  void savePointCloudCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
+                              std::shared_ptr<std_srvs::srv::Empty::Response>& response);
+
   void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
   void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
@@ -227,6 +235,9 @@ class OBCameraNode {
   void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                           const stream_index_pair& stream_index);
 
+  void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
+                       const sensor_msgs::msg::Image::SharedPtr& image_msg);
+
   bool setupFormatConvertType(OBFormat format);
 
  private:
@@ -318,5 +329,10 @@ class OBCameraNode {
   std::optional<OBCameraParam> camera_param_;
   bool enable_d2c_viewer_ = false;
   std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
+  std::map<stream_index_pair, std::atomic_bool> save_images_;
+  std::atomic_bool save_point_cloud_{false};
+  std::atomic_bool save_colored_point_cloud_{false};
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_point_cloud_srv_;
 };
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -16,6 +16,7 @@
 #include <geometry_msgs/msg/transform_stamped.hpp>
 
 #include "orbbec_camera/utils.h"
+#include <filesystem>
 namespace orbbec_camera {
 using namespace std::chrono_literals;
 
@@ -398,6 +399,20 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
   point_cloud_publisher_->publish(point_cloud_msg_);
+
+  if (save_point_cloud_) {
+    save_point_cloud_ = false;
+    auto now = std::time(nullptr);
+    std::stringstream ss;
+    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
+    auto current_path = std::filesystem::current_path().string();
+    std::string filename = current_path + "/point_cloud/points_" + ss.str() + ".ply";
+    if (!std::filesystem::exists(current_path + "/point_cloud")) {
+      std::filesystem::create_directory(current_path + "/point_cloud");
+    }
+    RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
+    savePointsToPly(frame, filename);
+  }
 }
 
 void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
@@ -463,6 +478,19 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
   colored_point_cloud_publisher_->publish(point_cloud_msg_);
+  if (save_colored_point_cloud_) {
+    save_colored_point_cloud_ = false;
+    auto now = std::time(nullptr);
+    std::stringstream ss;
+    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
+    auto current_path = std::filesystem::current_path().string();
+    std::string filename = current_path + "/point_cloud/colored_points_" + ss.str() + ".ply";
+    if (!std::filesystem::exists(current_path + "/point_cloud")) {
+      std::filesystem::create_directory(current_path + "/point_cloud");
+    }
+    RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
+    saveRGBPointsToPly(frame, filename);
+  }
 }
 
 void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set) {
@@ -548,6 +576,39 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
       depth_registration_ ? depth_aligned_frame_id_[stream_index] : optical_frame_id_[stream_index];
   CHECK(image_publishers_.count(stream_index) > 0);
   image_publishers_[stream_index].publish(image_msg);
+  saveImageToFile(stream_index, image, image_msg);
+}
+
+void OBCameraNode::saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
+                                   const sensor_msgs::msg::Image::SharedPtr& image_msg) {
+  if (save_images_[stream_index]) {
+    auto now = time(nullptr);
+    std::stringstream ss;
+    ss << std::put_time(localtime(&now), "%Y%m%d_%H%M%S");
+    auto current_path = std::filesystem::current_path().string();
+    auto fps = fps_[stream_index];
+    std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
+                           std::to_string(image_msg->width) + "x" +
+                           std::to_string(image_msg->height) + "_" + std::to_string(fps) + "hz_" +
+                           ss.str() + ".png";
+    if (!std::filesystem::exists(current_path + "/image")) {
+      std::filesystem::create_directory(current_path + "/image");
+    }
+    RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename);
+    if (stream_index.first == OB_STREAM_DEPTH) {
+      auto image_to_save = cv_bridge::toCvCopy(image_msg, encoding_[stream_index])->image;
+      cv::imwrite(filename, image_to_save);
+    } else if (stream_index.first == OB_STREAM_COLOR) {
+      auto image_to_save =
+          cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
+      cv::imwrite(filename, image_to_save);
+    } else if (stream_index.first == OB_STREAM_IR) {
+      cv::imwrite(filename, image);
+    } else {
+      RCLCPP_ERROR_STREAM(logger_, "Unsupported stream type: " << stream_index.first);
+    }
+    save_images_[stream_index] = false;
+  }
 }
 
 std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {

orbbec_camera/src/ros_service.cpp

@@ -140,6 +140,16 @@ void OBCameraNode::setupCameraCtrlServices() {
       "get_sdk_version",
       [this](const std::shared_ptr<GetString::Request> request,
              std::shared_ptr<GetString::Response> response) { getSDKVersion(request, response); });
+  save_images_srv_ = node_->create_service<std_srvs::srv::Empty>(
+      "save_images", [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+                            std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+        saveImageCallback(request, response);
+      });
+  save_point_cloud_srv_ = node_->create_service<std_srvs::srv::Empty>(
+      "save_point_cloud", [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+                                 std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+        savePointCloudCallback(request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -604,4 +614,29 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
     return false;
   }
 }
+
+void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
+                                     std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
+  (void)request;
+  (void)response;
+  for (const auto& stream_index : IMAGE_STREAMS) {
+    if (enable_stream_[stream_index]) {
+      save_images_[stream_index] = true;
+    }
+  }
+}
+
+void OBCameraNode::savePointCloudCallback(
+    const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
+    std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
+  (void)request;
+  (void)response;
+  if (enable_point_cloud_) {
+    save_point_cloud_ = true;
+  }
+  if (enable_colored_point_cloud_) {
+    save_colored_point_cloud_ = true;
+  }
+}
+
 }  // namespace orbbec_camera