feat: add bag recording control service

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -32,6 +32,7 @@
 #include <backward_ros/backward.hpp>
 #include "libobsensor/hpp/Device.hpp"
 #include "libobsensor/hpp/RecordPlayback.hpp"
+#include "orbbec_camera_msgs/srv/set_bag_recording.hpp"
 
 namespace orbbec_camera {
 
@@ -80,6 +81,10 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   void rebootDeviceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
                             std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+  void setBagRecordingCallback(
+      const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
+      std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response);
   void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
                             uint8_t percent);
   void updatePresetFirmware(std::string path);
@@ -139,6 +144,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::string timestamp_clock_type_str_;
   std::string preset_firmware_path_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
+  rclcpp::Service<orbbec_camera_msgs::srv::SetBagRecording>::SharedPtr set_bag_recording_srv_ =
+      nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   std::string extension_path_;
   static backward::SignalHandling sh;  // for stack trace

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -32,6 +32,7 @@
 #include <fstream>
 #include <iomanip>  // For std::put_time
 #include <malloc.h>
+#include <sstream>
 
 std::string g_camera_name = "orbbec_camera";  // Assuming this is declared elsewhere
 std::string g_time_domain = "global";         // Assuming this is declared elsewhere
@@ -45,6 +46,18 @@ std::string getLogDirectoryForCamera(const std::string &camera_name) {
   return (std::filesystem::path(home_dir) / ".ros" / "Log" / camera_name).string();
 }
 
+std::string getDefaultBagRecordFilePath() {
+  const std::time_t now_time = std::time(nullptr);
+  std::tm tm{};
+  localtime_r(&now_time, &tm);
+
+  std::ostringstream time_stream;
+  time_stream << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S");
+
+  return (std::filesystem::current_path() / ("orbbec_record_" + time_stream.str() + ".bag"))
+      .string();
+}
+
 std::string makeDefaultSdkLogFileName() {
   const std::time_t now_time = std::time(nullptr);
   std::tm tm{};
@@ -164,7 +177,7 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
   // Finalize bag recording before the pipeline is torn down, otherwise the
   // bag file can end up truncated/corrupted.
   if (record_device_) {
-    RCLCPP_INFO_STREAM(logger_, "Finalizing bag recording...");
+    RCLCPP_INFO_STREAM(logger_, "Stopping bag recording...");
     record_device_.reset();
   }
 
@@ -341,6 +354,9 @@ void OBCameraNodeDriver::init() {
   reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
       "reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
                                  std::placeholders::_1, std::placeholders::_2));
+  set_bag_recording_srv_ = this->create_service<orbbec_camera_msgs::srv::SetBagRecording>(
+      "set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this,
+                                     std::placeholders::_1, std::placeholders::_2));
   pthread_mutexattr_init(&orb_device_lock_attr_);
   pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
   orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
@@ -805,6 +821,54 @@ void OBCameraNodeDriver::deviceStatusTimer() {
   }
   // RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
 }
+
+void OBCameraNodeDriver::setBagRecordingCallback(
+    const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
+    std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response) {
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+
+  if (!device_) {
+    response->success = false;
+    response->message = "No device connected";
+    return;
+  }
+
+  if (!request->enable) {
+    if (!record_device_) {
+      response->success = true;
+      response->message = "Bag recording is not running";
+      return;
+    }
+    RCLCPP_INFO_STREAM(logger_, "Stopping bag recording");
+    record_device_.reset();
+    response->success = true;
+    response->message = "Bag recording stopped";
+    return;
+  }
+
+  std::string file_path =
+      request->file_path.empty() ? getDefaultBagRecordFilePath() : request->file_path;
+
+  if (record_device_) {
+    record_device_.reset();
+  }
+
+  try {
+    record_device_ =
+        std::make_shared<ob::RecordDevice>(device_, file_path, bag_record_compression_);
+  } catch (const ob::Error &e) {
+    response->success = false;
+    response->message = "Failed to start recording: " + orbbec_camera::formatObErrorWithStatus(e);
+    RCLCPP_ERROR_STREAM(logger_, response->message);
+    return;
+  }
+
+  RCLCPP_INFO_STREAM(logger_, "Recording to " << file_path);
+
+  response->success = true;
+  response->message = "Recording started: " + file_path;
+}
+
 void OBCameraNodeDriver::rebootDeviceCallback(
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response) {

orbbec_camera_msgs/CMakeLists.txt

@@ -36,6 +36,7 @@ rosidl_generate_interfaces(
     "srv/SetArrays.srv"
     "srv/GetUserCalibParams.srv"
     "srv/SetUserCalibParams.srv"
+    "srv/SetBagRecording.srv"
     DEPENDENCIES
     sensor_msgs
     std_msgs

orbbec_camera_msgs/srv/SetBagRecording.srv

@@ -0,0 +1,5 @@
+bool enable          # true starts recording, false stops recording
+string file_path     # path to save the .bag file when enable is true; empty uses current directory
+---
+bool success
+string message