Fix onTemperatureUpdate error

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -475,6 +475,8 @@ class OBCameraNode {
  private:
   std::atomic_bool write_customer_data_success_{false};
   std::atomic_bool user_calibration_ready_{false};
+  // New state flags to make clean() and rebootDevice() idempotent and thread-safe
+  std::atomic_bool cleaning_{false};
   rclcpp::Node* node_ = nullptr;
   std::shared_ptr<ob::Device> device_ = nullptr;
   std::shared_ptr<Parameters> parameters_ = nullptr;

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -16,6 +16,7 @@
 #pragma once
 #include <atomic>
 #include <thread>
+#include <chrono>
 
 #include <rclcpp/rclcpp.hpp>
 #include <semaphore.h>
@@ -108,6 +109,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::mutex reset_device_mutex_;
   std::condition_variable reset_device_cond_;
   std::atomic_bool reset_device_flag_{false};
+  std::chrono::steady_clock::time_point last_reset_device_completion_time_;
   pthread_mutex_t* orb_device_lock_ = nullptr;
   pthread_mutexattr_t orb_device_lock_attr_;
   uint8_t* orb_device_lock_shm_addr_ = nullptr;

orbbec_camera/src/ob_camera_node.cpp

@@ -128,6 +128,10 @@ void OBCameraNode::rebootDevice() {
 }
 
 void OBCameraNode::clean() noexcept {
+  if (cleaning_.exchange(true)) {
+    RCLCPP_DEBUG(logger_, "clean() already running, skip re-entry");
+    return;
+  }
   // Set running flag to false first to signal all operations to stop
   is_running_.store(false);
 
@@ -135,8 +139,14 @@ void OBCameraNode::clean() noexcept {
   try {
     if (diagnostic_timer_) {
       diagnostic_timer_->cancel();
+      // Wait for any currently executing timer callbacks to complete
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
       diagnostic_timer_.reset();
     }
+    if (software_trigger_timer_) {
+      software_trigger_timer_->cancel();
+      software_trigger_timer_.reset();
+    }
     if (diagnostic_updater_) {
       diagnostic_updater_.reset();
     }
@@ -195,6 +205,7 @@ void OBCameraNode::clean() noexcept {
   }
 
   RCLCPP_WARN_STREAM(logger_, "Do OBCameraNode clean DONE");
+  cleaning_.store(false);
 }
 
 void OBCameraNode::setupDevices() {
@@ -1993,13 +2004,26 @@ void OBCameraNode::setupTopics() {
 void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper &status) {
   try {
     // Check to ensure we're not shutting down and device is valid
-    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+    if (!is_running_.load() || !is_camera_node_initialized_.load()) {
-    if (!device_ || !is_running_.load() || !is_camera_node_initialized_.load()) {
       status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE,
                      "Device disconnected or shutting down");
       return;
     }
 
+    // Try to acquire device lock with timeout to avoid blocking during shutdown
+    std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock);
+    if (!lock.owns_lock()) {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE,
+                     "Device busy or shutting down");
+      return;
+    }
+
+    if (!device_) {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE,
+                     "Device not available");
+      return;
+    }
+
     // Additional safety check - verify device is actually accessible
     try {
       auto device_info = device_->getDeviceInfo();
@@ -2029,36 +2053,12 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
     status.add("Chip Bottom Temperature", temperature.chipBottomTemp);
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Temperature is normal");
   } catch (const ob::Error &e) {
-    try {
-      if (is_running_.load() && diagnostic_timer_) {
-        diagnostic_timer_->cancel();
-        diagnostic_timer_.reset();
-      }
-    } catch (...) {
-      // Ignore exceptions during cleanup
-    }
     RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate1: " << e.getMessage());
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.getMessage());
   } catch (const std::exception &e) {
-    try {
-      if (is_running_.load() && diagnostic_timer_) {
-        diagnostic_timer_->cancel();
-        diagnostic_timer_.reset();
-      }
-    } catch (...) {
-      // Ignore exceptions during cleanup
-    }
     RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate2: " << e.what());
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what());
   } catch (...) {
-    try {
-      if (is_running_.load() && diagnostic_timer_) {
-        diagnostic_timer_->cancel();
-        diagnostic_timer_.reset();
-      }
-    } catch (...) {
-      // Ignore exceptions during cleanup
-    }
     RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate3: Device is deactivated/disconnected!");
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown error");
   }
@@ -2078,12 +2078,32 @@ void OBCameraNode::setupDiagnosticUpdater() {
     diagnostic_timer_ =
         node_->create_wall_timer(std::chrono::seconds(int(diagnostic_period_)), [this]() {
           try {
-            if (is_running_.load() && diagnostic_updater_) {
+            // Check if we're still running and all components are valid
-              diagnostic_updater_->force_update();
+            if (!is_running_.load() || !diagnostic_updater_ ||
+                !is_camera_node_initialized_.load() || !device_) {
+              return;
             }
+
+            // Try to acquire device lock with timeout to avoid blocking during shutdown
+            std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock);
+            if (!lock.owns_lock()) {
+              // Device is busy or shutting down, skip this update
+              return;
+            }
+
+            diagnostic_updater_->force_update();
           } catch (const ob::Error &e) {
             RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: "
                                             << e.getMessage() << " - Device may be disconnected");
+            // Stop the diagnostic timer if device is having issues
+            try {
+              if (diagnostic_timer_) {
+                diagnostic_timer_->cancel();
+                diagnostic_timer_.reset();
+              }
+            } catch (...) {
+              // Ignore cleanup exceptions
+            }
           } catch (const std::exception &e) {
             RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: " << e.what());
           } catch (...) {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -223,6 +223,8 @@ void OBCameraNodeDriver::init() {
   orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
   pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
   is_alive_.store(true);
+  // Initialize the reset device completion time to allow immediate device connection on startup
+  last_reset_device_completion_time_ = std::chrono::steady_clock::now() - std::chrono::seconds(10);
   parameters_ = std::make_shared<Parameters>(this);
   serial_number_ = declare_parameter<std::string>("serial_number", "");
   device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
@@ -385,6 +387,18 @@ void OBCameraNodeDriver::queryDevice() {
 
     // If device is already connected, skip connection attempt
     if (!device_connected_.load()) {
+      // Check if sufficient time has passed since last reset device completion
+      auto now = std::chrono::steady_clock::now();
+      auto time_since_last_reset = std::chrono::duration_cast<std::chrono::seconds>(
+          now - last_reset_device_completion_time_);
+
+      if (time_since_last_reset.count() < 10) {
+        RCLCPP_DEBUG_STREAM(logger_, "queryDevice: Only " << time_since_last_reset.count()
+                           << " seconds since last reset completion, waiting before starting device...");
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        continue;
+      }
+
       if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
         connectNetDevice(net_device_ip_, net_device_port_);
       } else {
@@ -462,8 +476,12 @@ void OBCameraNodeDriver::resetDevice() {
             }
             device_.reset();
             RCLCPP_INFO_STREAM(logger_, "device_ reset completed");
+          } catch (const ob::Error& e) {
+            RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: " << e.getMessage());
+          } catch (const std::exception& e) {
+            RCLCPP_WARN_STREAM(logger_, "Standard exception during device reset: " << e.what());
           } catch (...) {
-            RCLCPP_WARN_STREAM(logger_, "Exception during device reset");
+            RCLCPP_WARN_STREAM(logger_, "Unknown exception during device reset");
           }
         }
 
@@ -480,6 +498,7 @@ void OBCameraNodeDriver::resetDevice() {
         device_unique_id_.clear();
       }
       reset_device_flag_ = false;
+      last_reset_device_completion_time_ = std::chrono::steady_clock::now();
     }
     reset_device_cond_.notify_all();
     malloc_trim(0);