Fix device disconnection after reboot service call

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -158,6 +158,13 @@ class OBCameraNode {
 
   void clean() noexcept;
 
+  // Safely expose the lock
+  template <typename Func>
+  auto withDeviceLock(Func &&func) -> decltype(func()) {
+    std::lock_guard<std::recursive_mutex> lock(device_lock_);
+    return func();
+  }
+
   void rebootDevice();
 
   void startStreams();
@@ -618,7 +625,7 @@ class OBCameraNode {
   std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
   uint8_t* rgb_buffer_ = nullptr;
   bool is_color_frame_decoded_ = false;
-  std::mutex device_lock_;
+  std::recursive_mutex device_lock_;
   // For color
   std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
   std::shared_ptr<std::thread> colorFrameThread_ = nullptr;

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -118,5 +118,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::string extension_path_;
   static backward::SignalHandling sh;  // for stack trace
   std::string upgrade_firmware_;
+  std::atomic<bool> firmware_update_success_{false};
 };
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -113,7 +113,7 @@ OBCameraNode::~OBCameraNode() noexcept { clean(); }
 void OBCameraNode::rebootDevice() {
   RCLCPP_INFO_STREAM(logger_, "Do clean before rebooting device");
   malloc_trim(0);
-  clean();
+  stopStreams();
   malloc_trim(0);
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
   RCLCPP_INFO_STREAM(logger_, "Reboot device");

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -557,13 +557,23 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start_time_);
   RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
+
   if (!upgrade_firmware_.empty()) {
-    device_->updateFirmware(
+    firmware_update_success_ = false;
-        upgrade_firmware_.c_str(),
+
-        std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
+    ob_camera_node_->withDeviceLock([&]() {
-                  std::placeholders::_2, std::placeholders::_3),
+      device_->updateFirmware(
-        false);
+          upgrade_firmware_.c_str(),
+          std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
+                    std::placeholders::_2, std::placeholders::_3),
+          false);
+    });
+    if(firmware_update_success_)
+    {
+      return;
+    }
   }
+
   if (ob_camera_node_) {
     ob_camera_node_->startIMU();
     ob_camera_node_->startStreams();
@@ -805,9 +815,11 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
   std::cout << "Message : " << message << std::endl << std::flush;
   if (state == STAT_DONE) {
     RCLCPP_INFO(logger_, "Reboot device");
-    ob_camera_node_->rebootDevice();
+    device_->reboot();
     device_connected_ = false;
     upgrade_firmware_ = "";
+
+    firmware_update_success_ = true;
   }
 }
 }  // namespace orbbec_camera