fix: restart playback streams on loop

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -192,6 +192,8 @@ class OBCameraNode {
 
   void startIMU();
 
+  void restartPlaybackStreams();
+
   int openSocSyncPwmTrigger(uint16_t fps);
   int closeSocSyncPwmTrigger();
   void startGmslTrigger();

orbbec_camera/src/ob_camera_node.cpp

@@ -3303,6 +3303,26 @@ void OBCameraNode::startIMU() {
   }
 }
 
+void OBCameraNode::restartPlaybackStreams() {
+  if (!is_playback_device_ || !is_running_.load()) {
+    return;
+  }
+
+  try {
+    stopStreams();
+    startStreams();
+    stopIMU();
+    startIMU();
+  } catch (const ob::Error &e) {
+    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: "
+                                    << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception &e) {
+    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: " << e.what());
+  } catch (...) {
+    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams");
+  }
+}
+
 void OBCameraNode::stopStreams() {
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
 

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -1023,7 +1023,7 @@ void OBCameraNodeDriver::initializeBagPlayback() {
         try {
           playback_device_->seek(0);
           if (ob_camera_node_) {
-            ob_camera_node_->startStreams();
+            ob_camera_node_->restartPlaybackStreams();
           }
         } catch (const ob::Error &e) {
           RCLCPP_WARN_STREAM(logger_, "Failed to restart bag playback: "