Add the possibility to use a bag file instead of a physical device

A bag_filename can now be specified as parameter to the
orbbec_camera_node, that will be replayed using the PlaybackDevice
interface.

A bag_replay_loop boolean parameter is added to allow replaying the bag
in a loop.

The format of the bag file is the one generated when recording from the
OrbbecViewer tool.

Author: milidam

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -61,6 +61,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   void connectNetDevice(const std::string& net_device_ip, int net_device_port);
 
+  void openPlaybackDevice(const std::string &bagfile_path, bool bag_replay_loop);
+
   void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list);
 
   void onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list);
@@ -128,6 +130,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
   // net config
   std::string net_device_ip_;
   int net_device_port_ = 0;
+  std::string bag_filename_;
+  bool bag_replay_loop_ = false;
   int connection_delay_ = 100;
   bool enable_sync_host_time_ = true;
   std::chrono::milliseconds time_sync_period_{6000};

orbbec_camera/include/orbbec_camera/utils.h

@@ -222,4 +222,6 @@ double getScanAngleIncrement(OBLiDARScanRate fps);
 double deg2rad(double deg);
 
 double rad2deg(double rad);
+
+bool is_physical_device(std::shared_ptr<ob::Device> device);
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini_330_series.launch.py

@@ -188,6 +188,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enumerate_net_device', default_value='true'),
         DeclareLaunchArgument('net_device_ip', default_value=''),
         DeclareLaunchArgument('net_device_port', default_value='0'),
+        DeclareLaunchArgument('bag_filename', default_value=''),
+        DeclareLaunchArgument('bag_replay_loop', default_value='false'),
         DeclareLaunchArgument('device_access_mode', default_value='Default'), # Default, EA or CA . only for 335le
         DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular
         DeclareLaunchArgument('log_level', default_value='none'),

orbbec_camera/src/ob_camera_node.cpp

@@ -443,7 +443,7 @@ void OBCameraNode::setupDevices() {
     RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
     TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
   }
-  if (!sync_mode_str_.empty()) {
+  if (!sync_mode_str_.empty() && is_physical_device(device_)) {
     auto sync_config = device_->getMultiDeviceSyncConfig();
     RCLCPP_INFO_STREAM(logger_,
                        "Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
@@ -2225,7 +2225,7 @@ void OBCameraNode::getParameters() {
   if (isOpenNIDevice(pid_)) {
     time_domain_ = "system";
   }
-  if (time_domain_ == "global") {
+  if (time_domain_ == "global" && is_physical_device(device_)) {
     device_->enableGlobalTimestamp(true);
   }
   setAndGetNodeParameter<int>(frames_per_trigger_, "frames_per_trigger", 2);
@@ -2405,7 +2405,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
 }
 
 void OBCameraNode::setupDiagnosticUpdater() {
-  if (diagnostic_period_ <= 0.0) {
+  if (diagnostic_period_ <= 0.0 || !is_physical_device(device_)) {
     return;
   }
   try {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -275,6 +275,8 @@ void OBCameraNodeDriver::init() {
   usb_port_ = declare_parameter<std::string>("usb_port", "");
   net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
   net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
+  bag_filename_ = declare_parameter<std::string>("bag_filename", "");
+  bag_replay_loop_ = static_cast<int>(declare_parameter<bool>("bag_replay_loop", true));
   enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
   uvc_backend_ = declare_parameter<std::string>("uvc_backend", "libuvc");
   device_access_mode_str_ = declare_parameter<std::string>("device_access_mode", "Default");
@@ -456,6 +458,8 @@ void OBCameraNodeDriver::queryDevice() {
 
       if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
         connectNetDevice(net_device_ip_, net_device_port_);
+      } else if (!bag_filename_.empty()) {
+          openPlaybackDevice(bag_filename_, bag_replay_loop_);
       } else {
         auto device_list = ctx_->queryDeviceList();
         if (device_list->getCount() != 0) {
@@ -1227,6 +1231,51 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
   }
 }
 
+void OBCameraNodeDriver::openPlaybackDevice(const std::string &bagfile_path, bool bag_replay_loop) {
+  if (bagfile_path.empty() || !std::filesystem::exists(bagfile_path)) {
+    RCLCPP_ERROR_STREAM(logger_, "Invalid bag file path " << bagfile_path);
+    return;
+  }
+
+  auto playback_device = std::make_shared<ob::PlaybackDevice>(bagfile_path.c_str());
+  if (playback_device == nullptr) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open playback device " << bagfile_path);
+    return;
+  }
+  try {
+    RCLCPP_INFO_STREAM(logger_, "Opening bag file " << bagfile_path);
+
+    // Automatically restart playback when reaching file end
+    playback_device->setPlaybackStatusChangeCallback([&](OBPlaybackStatus status)
+    {
+        RCLCPP_INFO(logger_, "Playback status changed to %d", status);
+
+        if (status == OB_PLAYBACK_STOPPED) {
+            if (bag_replay_loop && is_alive_ && rclcpp::ok()) {
+                // restart bag file
+                ob_camera_node_->clean();
+                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+                initializeDevice(playback_device);
+            } else {
+                // stop it here
+                rclcpp::shutdown();
+            }
+        }
+    });
+
+    initializeDevice(playback_device);
+    if (!device_connected_) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize playback device " << bagfile_path);
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR_STREAM(logger_, "Exception during playback device initialization: " << e.what());
+    device_connected_ = false;
+  } catch (...) {
+    RCLCPP_ERROR_STREAM(logger_, "Unknown exception during playback device initialization");
+    device_connected_ = false;
+  }
+}
+
 void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
   if (device_connected_.load()) {
     return;

orbbec_camera/src/utils.cpp

@@ -1016,4 +1016,10 @@ double rad2deg(double rad) {
   return angle_degrees;
 }
 
+bool is_physical_device(std::shared_ptr<ob::Device> device)
+{
+    // any non PlaybackDevice is supposed to be a physical one
+    return (dynamic_cast<ob::PlaybackDevice*>(device.get()) == nullptr);
+}
+
 }  // namespace orbbec_camera