Fix firmware update and avoid colcon symlink issues

Author: xiexun

orbbec_camera/CMakeLists.txt

@@ -228,7 +228,11 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY examples DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY ${ORBBEC_INCLUDE_DIR} DESTINATION include)
 install(DIRECTORY ${ORBBEC_LIBS_DIR}/ DESTINATION lib/ FILES_MATCHING PATTERN "*.so*")
-install(DIRECTORY ${ORBBEC_LIBS_DIR}/extensions DESTINATION lib/)
+# Install extensions directory with explicit file copying to avoid symlink issues
+file(GLOB_RECURSE EXTENSION_FILES "${ORBBEC_LIBS_DIR}/extensions/*")
+if(EXTENSION_FILES)
+    install(FILES ${EXTENSION_FILES} DESTINATION lib/extensions/)
+endif()
 if(DEFINED ENV{BUILDING_PACKAGE})
     # Install udev rules
     install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/99-obsensor-libusb.rules DESTINATION /etc/udev/rules.d)

orbbec_camera/src/ob_camera_node.cpp

@@ -1940,11 +1940,22 @@ void OBCameraNode::setupTopics() {
 
 void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper &status) {
   try {
-
-    //check to ensure we're not shutting down
+    // Check to ensure we're not shutting down and device is valid
     std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-    if (!device_ || !is_running_.load()) {
-      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device disconnected");
+    if (!device_ || !is_running_.load() || !is_camera_node_initialized_.load()) {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device disconnected or shutting down");
+      return;
+    }
+
+    // Additional safety check - verify device is actually accessible
+    try {
+      auto device_info = device_->getDeviceInfo();
+      if (!device_info) {
+        status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device info not available");
+        return;
+      }
+    } catch (...) {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device not accessible");
       return;
     }
 
@@ -1995,7 +2006,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
     } catch (...) {
       // Ignore exceptions during cleanup
     }
-    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate");
+    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate: Device is deactivated/disconnected!");
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown error");
   }
 }

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -36,39 +36,43 @@ std::string g_time_domain = "global";         // Assuming this is declared elsew
 
 void signalHandler(int sig) {
   std::cout << "Received signal: " << sig << std::endl;
+  if (sig == SIGINT || sig == SIGTERM) {
+    rclcpp::shutdown();
+  } else {
 
-  std::string log_dir = "Log/";
+    std::string log_dir = "Log/";
 
-  // get current time
-  std::time_t now = std::time(nullptr);
-  std::tm *local_time = std::localtime(&now);
+    // get current time
+    std::time_t now = std::time(nullptr);
+    std::tm *local_time = std::localtime(&now);
 
-  // format date and time to string, format as "2024_05_20_12_34_56"
-  std::ostringstream time_stream;
-  time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");
+    // format date and time to string, format as "2024_05_20_12_34_56"
+    std::ostringstream time_stream;
+    time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");
 
-  // generate log file name
-  std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
-  std::string log_file_path = log_dir + log_file_name;
+    // generate log file name
+    std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
+    std::string log_file_path = log_dir + log_file_name;
 
-  if (!std::filesystem::exists(log_dir)) {
-    std::filesystem::create_directories(log_dir);
-  }
+    if (!std::filesystem::exists(log_dir)) {
+      std::filesystem::create_directories(log_dir);
+    }
 
-  std::cout << "Log crash stack trace to " << log_file_path << std::endl;
-  std::ofstream log_file(log_file_path, std::ios::app);
+    std::cout << "Log crash stack trace to " << log_file_path << std::endl;
+    std::ofstream log_file(log_file_path, std::ios::app);
 
-  if (log_file.is_open()) {
-    log_file << "Received signal: " << sig << std::endl;
+    if (log_file.is_open()) {
+      log_file << "Received signal: " << sig << std::endl;
 
-    backward::StackTrace st;
-    st.load_here(32);  // Capture stack
-    backward::Printer p;
-    p.print(st, log_file);  // Print stack to log file
-  }
+      backward::StackTrace st;
+      st.load_here(32);  // Capture stack
+      backward::Printer p;
+      p.print(st, log_file);  // Print stack to log file
+    }
 
-  log_file.close();
-  exit(sig);  // Exit program
+    log_file.close();
+    exit(sig);  // Exit program
+  }
 }
 
 namespace orbbec_camera {
@@ -97,10 +101,28 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
 OBCameraNodeDriver::~OBCameraNodeDriver() {
   is_alive_.store(false);
 
+  // First stop the camera node cleanly before stopping threads
+  if (ob_camera_node_) {
+    try {
+      ob_camera_node_->clean();
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Exception during camera node cleanup in destructor");
+    }
+    ob_camera_node_->stopGmslTrigger();
+  }
+
+  // Stop timers that might access the device
+  if (sync_host_time_timer_) {
+    sync_host_time_timer_->cancel();
+    sync_host_time_timer_.reset();
+  }
+
   if (check_connect_timer_) {
+    check_connect_timer_->cancel();
     check_connect_timer_.reset();
   }
 
+  // Now stop threads
   if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
     device_count_update_thread_->join();
   }
@@ -111,9 +133,8 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
     reset_device_cond_.notify_all();
     reset_device_thread_->join();
   }
-  if (ob_camera_node_) {
-    ob_camera_node_->stopGmslTrigger();
-  }
+
+  // Clean up shared memory
   if (orb_device_lock_shm_fd_ != -1) {
     close(orb_device_lock_shm_fd_);
     orb_device_lock_shm_fd_ = -1;
@@ -131,6 +152,8 @@ void OBCameraNodeDriver::init() {
   signal(SIGABRT, signalHandler);  // abort
   signal(SIGFPE, signalHandler);   // float point exception
   signal(SIGILL, signalHandler);   // illegal instruction
+  signal(SIGINT, signalHandler);
+  signal(SIGTERM, signalHandler);
   ob::Context::setExtensionsDirectory(extension_path_.c_str());
   if (config_path_.empty()) {
     ctx_ = std::make_unique<ob::Context>();
@@ -301,6 +324,17 @@ void OBCameraNodeDriver::resetDevice() {
     RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
     std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
     {
+      // First stop the camera node cleanly to prevent timer/diagnostic access to device
+      if (ob_camera_node_) {
+        try {
+          // This will stop all timers and clean up properly
+          ob_camera_node_->clean();
+        } catch (...) {
+          RCLCPP_WARN_STREAM(logger_, "Exception during camera node cleanup during reset");
+        }
+      }
+
+      // Now reset the objects
       ob_camera_node_.reset();
       device_.reset();
       device_info_.reset();
@@ -817,6 +851,18 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
   std::cout << "Message : " << message << std::endl << std::flush;
   if (state == STAT_DONE) {
     RCLCPP_INFO(logger_, "Reboot device");
+
+    // Don't call clean() here to avoid deadlock - just stop timers and reboot
+    // The resetDevice thread will handle proper cleanup when device disconnects
+    if (sync_host_time_timer_) {
+      try {
+        sync_host_time_timer_->cancel();
+        sync_host_time_timer_.reset();
+      } catch (...) {
+        // Ignore exceptions during timer cleanup
+      }
+    }
+
     device_->reboot();
     device_connected_ = false;
     upgrade_firmware_ = "";