Fix crash by improving lock handling

Author: xiexun

orbbec_camera/include/orbbec_camera/d2c_viewer.h

@@ -42,5 +42,9 @@ class D2CViewer {
                                                                        sensor_msgs::msg::Image>;
   std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> sync_;
   rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr d2c_viewer_pub_;
+
+  // Add thread safety
+  std::mutex callback_mutex_;
+  std::atomic<bool> is_active_{true};
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -87,6 +87,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::shared_ptr<ob::DeviceInfo> device_info_ = nullptr;
   std::atomic_bool is_alive_{false};
   std::atomic_bool device_connected_{false};
+  std::atomic_bool device_connecting_{false};
   std::string serial_number_;
   std::string device_unique_id_;
   std::string usb_port_;

orbbec_camera/src/d2c_viewer.cpp

@@ -28,7 +28,7 @@
 namespace orbbec_camera {
 D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
                      rmw_qos_profile_t depth_qos)
-    : node_(node), logger_(rclcpp::get_logger("d2c_viewer")) {
+    : node_(node), logger_(rclcpp::get_logger("d2c_viewer")), is_active_(true) {
   rgb_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
       node_, "color/image_raw", rgb_qos);
   depth_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
@@ -43,10 +43,36 @@ D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
   d2c_viewer_pub_ =
       node_->create_publisher<sensor_msgs::msg::Image>("depth_to_color/image_raw", rclcpp::QoS(1));
 }
-D2CViewer::~D2CViewer() = default;
+
+D2CViewer::~D2CViewer() {
+  is_active_.store(false);
+
+  // Safely shut down subscribers and synchronizer
+  {
+    std::lock_guard<std::mutex> lock(callback_mutex_);
+    if (sync_) {
+      sync_.reset();
+    }
+    if (rgb_sub_) {
+      rgb_sub_.reset();
+    }
+    if (depth_sub_) {
+      depth_sub_.reset();
+    }
+    if (d2c_viewer_pub_) {
+      d2c_viewer_pub_.reset();
+    }
+  }
+}
 
 void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
                                 const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) {
+
+  std::lock_guard<std::mutex> lock(callback_mutex_);
+  if (!is_active_.load()) {
+    return;
+  }
+
   if (rgb_msg->width != depth_msg->width || rgb_msg->height != depth_msg->height) {
     RCLCPP_ERROR(logger_, "rgb and depth image size not match(%d, %d) vs (%d, %d)", rgb_msg->width,
                  rgb_msg->height, depth_msg->width, depth_msg->height);

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);
-  stopStreams();
+  clean();
   malloc_trim(0);
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
   RCLCPP_INFO_STREAM(logger_, "Reboot device");
@@ -125,11 +125,10 @@ void OBCameraNode::rebootDevice() {
 }
 
 void OBCameraNode::clean() noexcept {
-  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-  RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
+  // Set running flag to false first to signal all operations to stop
   is_running_.store(false);
 
-  // Stop diagnostic timer and updater first to prevent access to disconnected device
+  // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
   try {
     if (diagnostic_timer_) {
       diagnostic_timer_->cancel();
@@ -142,6 +141,10 @@ void OBCameraNode::clean() noexcept {
     // Ignore exceptions during diagnostic cleanup
   }
 
+  // Now acquire the device lock for the rest of the cleanup
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
+
   RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
   try {
     if (tf_thread_ && tf_thread_->joinable()) {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -239,6 +239,13 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
   if (device_list->getCount() == 0) {
     return;
   }
+
+  // Check if device is already connected or connecting
+  if (device_connected_.load() || device_connecting_.load()) {
+    RCLCPP_DEBUG_STREAM(logger_, "onDeviceConnected: device already connected or connecting");
+    return;
+  }
+
   if (!device_) {
     startDevice(device_list);
   }
@@ -250,17 +257,28 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
     return;
   }
   RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
+
+  // Check if device connection/initialization is in progress
+  if (device_connecting_.load()) {
+    RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected: device connection/initialization in progress, ignoring disconnect event");
+    return;
+  }
+
+  std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+
   for (size_t i = 0; i < device_list->getCount(); i++) {
     std::string uid = device_list->getUid(i);
     std::string serial_number = device_list->getSerialNumber(i);
-    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
     RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
     if (uid == device_unique_id_ || serial_number_ == serial_number) {
       RCLCPP_INFO_STREAM(logger_,
-                         "device with " << uid << " disconnected, notify reset device thread.");
-      std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
+                         "device with " << uid << " disconnected, notify reset device thread 1.");
       reset_device_flag_ = true;
       reset_device_cond_.notify_all();
+      RCLCPP_INFO_STREAM(logger_,
+                         "device with " << uid << " disconnected, notify reset device thread 2.");
       break;
     }
   }
@@ -294,6 +312,26 @@ void OBCameraNodeDriver::checkConnectTimer() {
 
 void OBCameraNodeDriver::queryDevice() {
   while (is_alive_ && rclcpp::ok() && !device_connected_.load()) {
+    // Check if device reset is in progress before attempting to connect
+    {
+      std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
+      if (reset_device_flag_) {
+        RCLCPP_INFO_STREAM(logger_, "queryDevice: device reset in progress, waiting...");
+        reset_device_cond_.wait(reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
+        if (!is_alive_ || !rclcpp::ok()) {
+          return;
+        }
+        RCLCPP_INFO_STREAM(logger_, "queryDevice: device reset completed, continuing connection");
+      }
+    }
+
+    // Check if connection is already in progress
+    if (device_connecting_.load()) {
+      RCLCPP_DEBUG_STREAM(logger_, "queryDevice: device connection already in progress, waiting...");
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      continue;
+    }
+
     if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
       connectNetDevice(net_device_ip_, net_device_port_);
     } else {
@@ -306,6 +344,9 @@ void OBCameraNodeDriver::queryDevice() {
       }
       startDevice(device_list);
     }
+
+    // Add a small delay to prevent tight loop
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
 }
 
@@ -330,11 +371,52 @@ void OBCameraNodeDriver::resetDevice() {
         }
       }
 
-      // Now reset the objects
-      ob_camera_node_.reset();
-      device_.reset();
-      device_info_.reset();
+      // Mark device as disconnected immediately to prevent other threads from accessing it
       device_connected_ = false;
+      device_connecting_ = false;  // Clear connecting flag
+
+      // Reset objects in order, with additional safety checks
+      if (ob_camera_node_) {
+        try {
+          RCLCPP_INFO_STREAM(logger_, "Resetting ob_camera_node_");
+          ob_camera_node_.reset();
+          RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ reset completed");
+        } catch (...) {
+          RCLCPP_WARN_STREAM(logger_, "Exception during ob_camera_node reset");
+        }
+      }
+
+      // Allow more time for internal SDK cleanup
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+      if (device_) {
+        try {
+          RCLCPP_INFO_STREAM(logger_, "Resetting device_");
+          // Force free any idle memory before device reset
+          if (ctx_) {
+            try {
+              ctx_->freeIdleMemory();
+            } catch (...) {
+              // Ignore exceptions during memory cleanup
+            }
+          }
+          device_.reset();
+          RCLCPP_INFO_STREAM(logger_, "device_ reset completed");
+        } catch (...) {
+          RCLCPP_WARN_STREAM(logger_, "Exception during device reset");
+        }
+      }
+
+      if (device_info_) {
+        try {
+          RCLCPP_INFO_STREAM(logger_, "Resetting device_info_");
+          device_info_.reset();
+          RCLCPP_INFO_STREAM(logger_, "device_info_ reset completed");
+        } catch (...) {
+          RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
+        }
+      }
+
       device_unique_id_.clear();
     }
     reset_device_flag_ = false;
@@ -620,19 +702,57 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
     RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
     return;
   }
+
+  // Check if already connecting
+  bool expected = false;
+  if (!device_connecting_.compare_exchange_strong(expected, true)) {
+    RCLCPP_DEBUG_STREAM(logger_, "connectNetDevice: connection already in progress");
+    return;
+  }
+
+  // Use RAII to ensure connecting flag is cleared
+  std::shared_ptr<int> connecting_guard(nullptr, [this](int *) {
+    device_connecting_.store(false);
+  });
+
   std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
   auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port);
   if (device == nullptr) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to connect to net device " << net_device_ip);
     return;
   }
-  initializeDevice(device);
+  try {
+    initializeDevice(device);
+    if (!device_connected_) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to initialize net device " << net_device_ip);
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR_STREAM(logger_, "Exception during net device initialization: " << e.what());
+    device_connected_ = false;
+  } catch (...) {
+    RCLCPP_ERROR_STREAM(logger_, "Unknown exception during net device initialization");
+    device_connected_ = false;
+  }
 }
 
 void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
-  if (device_connected_) {
+  if (device_connected_.load()) {
+    return;
+  }
+
+  // Try to set connecting flag atomically
+  bool expected = false;
+  if (!device_connecting_.compare_exchange_strong(expected, true)) {
+    RCLCPP_DEBUG_STREAM(logger_, "startDevice: connection already in progress by another thread");
     return;
   }
+
+  // Use RAII to ensure connecting flag is cleared
+  std::shared_ptr<int> connecting_guard(nullptr, [this](int *) {
+    device_connecting_.store(false);
+    RCLCPP_DEBUG_STREAM(logger_, "startDevice: connecting flag cleared");
+  });
+
   if (list->getCount() == 0) {
     RCLCPP_WARN(logger_, "No device found");
     return;
@@ -687,6 +807,16 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
     time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
     RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
 
+    if(firmware_update_success_)
+    {
+      firmware_update_success_ = false;
+      device_connected_ = false;
+      std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
+      reset_device_flag_ = true;
+      reset_device_cond_.notify_all();
+      return;
+    }
+
     auto pid = device->getDeviceInfo()->getPid();
     if (GEMINI_335LG_PID == pid) {
       ob_camera_node_->startGmslTrigger();