Fix crash and change parameter order in gemini435_le launch file by repositioning time_sync_period argument

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -193,13 +193,6 @@ class OBCameraNode {
 
   void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
     fps_delay_status_depth_->fillDepthStatus(status_msg);
-    status_msg.header.frame_id = camera_link_frame_id_;
-  }
-
-  void publishDeviceStatus(const orbbec_camera_msgs::msg::DeviceStatus& msg) {
-    if (device_status_pub_) {
-      device_status_pub_->publish(msg);
-    }
   }
 
   bool checkUserCalibrationReady() {
@@ -245,8 +238,6 @@ class OBCameraNode {
 
   void setupDiagnosticUpdater();
 
-  void setupPeriodicHostTimeSync();
-
   void onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& status);
 
   void setupCameraCtrlServices();
@@ -776,9 +767,7 @@ class OBCameraNode {
   // soft ware trigger
   rclcpp::TimerBase::SharedPtr software_trigger_timer_;
   rclcpp::TimerBase::SharedPtr diagnostic_timer_;
-  rclcpp::TimerBase::SharedPtr sync_timer_;
   std::chrono::milliseconds software_trigger_period_{33};
-  std::chrono::milliseconds time_sync_period_{6000};
   bool enable_heartbeat_ = false;
   bool enable_color_undistortion_ = false;
   std::shared_ptr<image_publisher> color_undistortion_publisher_;
@@ -835,6 +824,5 @@ class OBCameraNode {
 
   std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
   std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
-  rclcpp::Publisher<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_pub_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -22,6 +22,7 @@
 #include "ob_camera_node.h"
 #include "utils.h"
 #include "dynamic_params.h"
+#include <orbbec_camera_msgs/msg/device_status.hpp>
 
 #include "libobsensor/ObSensor.hpp"
 #include <pthread.h>
@@ -116,6 +117,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   int net_device_port_ = 0;
   int connection_delay_ = 100;
   bool enable_sync_host_time_ = true;
+  std::chrono::milliseconds time_sync_period_{6000};
   std::string preset_firmware_path_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
@@ -125,5 +127,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::atomic<bool> firmware_update_success_{false};
   rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr;
   int device_status_interval_hz = 2;  // 2Hz
+  rclcpp::Publisher<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_pub_ = nullptr;
+  std::string node_name_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/utils.h

@@ -32,7 +32,6 @@
 #include <iomanip>
 #include <arpa/inet.h>
 
-
 namespace orbbec_camera {
 inline void LogFatal(const char* file, int line, const std::string& message) {
   std::cerr << "Check failed at " << file << ":" << line << ": " << message << std::endl;
@@ -40,15 +39,25 @@ inline void LogFatal(const char* file, int line, const std::string& message) {
 }
 }  // namespace orbbec_camera
 
-#define TRY_EXECUTE_BLOCK(block)                                                                 \
+#define TRY_EXECUTE_BLOCK(block)                                                                  \
-  try {                                                                                          \
+  try {                                                                                           \
-    block;                                                                                       \
+    block;                                                                                        \
-  } catch (const ob::Error& e) {                                                                 \
+  } catch (const ob::Error& e) {                                                                  \
-    RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__, e.getMessage()); \
+    std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";                 \
-  } catch (const std::exception& e) {                                                            \
+    if (error_msg.find("Device is deactivated") != std::string::npos ||                           \
-    RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());   \
+        error_msg.find("disconnected") != std::string::npos ||                                    \
-  } catch (...) {                                                                                \
+        error_msg.find("Send control transfer failed") != std::string::npos) {                    \
-    RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);         \
+      RCLCPP_WARN(logger_,                                                                        \
+                  "Device communication error in %s at line %d: %s - Device may be disconnected", \
+                  __FUNCTION__, __LINE__, error_msg.c_str());                                     \
+    } else {                                                                                      \
+      RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,                 \
+                   error_msg.c_str());                                                            \
+    }                                                                                             \
+  } catch (const std::exception& e) {                                                             \
+    RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());    \
+  } catch (...) {                                                                                 \
+    RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);          \
   }
 
 #define TRY_TO_SET_PROPERTY(func, property, value)                                             \
@@ -200,5 +209,5 @@ cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
 
 std::string getDistortionModels(OBCameraDistortion distortion);
 
-std::string calcMD5(const std::string &data);
+std::string calcMD5(const std::string& data);
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini435_le.launch.py

@@ -240,13 +240,13 @@ def generate_launch_description():
         DeclareLaunchArgument('align_mode', default_value='HW'),
         DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH
         DeclareLaunchArgument('diagnostic_period', default_value='1.0'), # seconds
-        DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('enable_laser', default_value='true'),
         DeclareLaunchArgument('depth_precision', default_value=''),
         DeclareLaunchArgument('device_preset', default_value='Default'),
         DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
+        DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),

orbbec_camera/src/ob_camera_node.cpp

@@ -1885,9 +1885,6 @@ void OBCameraNode::getParameters() {
   int software_trigger_period = 33;
   setAndGetNodeParameter<int>(software_trigger_period, "software_trigger_period", 33);
   software_trigger_period_ = std::chrono::milliseconds(software_trigger_period);
-  double time_sync_period = 6.0;
-  setAndGetNodeParameter<double>(time_sync_period, "time_sync_period", 6.0);
-  time_sync_period_ = std::chrono::milliseconds((int)(time_sync_period * 1000));
   setAndGetNodeParameter<int>(gmsl_trigger_fps_, "gmsl_trigger_fps", 3000);
   setAndGetNodeParameter<bool>(enable_gmsl_trigger_, "enable_gmsl_trigger", false);
   setAndGetNodeParameter<std::string>(interleave_ae_mode_, "interleave_ae_mode", "hdr");
@@ -1974,7 +1971,6 @@ void OBCameraNode::setupTopics() {
     setupCameraCtrlServices();
     setupPublishers();
     setupDiagnosticUpdater();
-    setupPeriodicHostTimeSync();
   } catch (const ob::Error &e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
     throw std::runtime_error(e.getMessage());
@@ -2034,7 +2030,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
     } catch (...) {
       // Ignore exceptions during cleanup
     }
-    RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate: " << e.getMessage());
+    RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate1: " << e.getMessage());
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.getMessage());
   } catch (const std::exception &e) {
     try {
@@ -2045,7 +2041,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
     } catch (...) {
       // Ignore exceptions during cleanup
     }
-    RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate: " << e.what());
+    RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate2: " << e.what());
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what());
   } catch (...) {
     try {
@@ -2056,7 +2052,7 @@ void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapp
     } catch (...) {
       // Ignore exceptions during cleanup
     }
-    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate: Device is deactivated/disconnected!");
+    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate3: Device is deactivated/disconnected!");
     status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown error");
   }
 }
@@ -2072,8 +2068,21 @@ void OBCameraNode::setupDiagnosticUpdater() {
     diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(node_, 10000.0);
     diagnostic_updater_->setHardwareID(serial_number);
     diagnostic_updater_->add("Temperatures", this, &OBCameraNode::onTemperatureUpdate);
-    diagnostic_timer_ = node_->create_wall_timer(std::chrono::seconds(int(diagnostic_period_)),
+    diagnostic_timer_ =
-                                                 [this]() { diagnostic_updater_->force_update(); });
+        node_->create_wall_timer(std::chrono::seconds(int(diagnostic_period_)), [this]() {
+          try {
+            if (is_running_.load() && diagnostic_updater_) {
+              diagnostic_updater_->force_update();
+            }
+          } catch (const ob::Error &e) {
+            RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: "
+                                            << e.getMessage() << " - Device may be disconnected");
+          } catch (const std::exception &e) {
+            RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: " << e.what());
+          } catch (...) {
+            RCLCPP_WARN(logger_, "Diagnostic update failed: Unknown error");
+          }
+        });
   } catch (const ob::Error &e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: " << e.getMessage());
   } catch (const std::exception &e) {
@@ -2083,35 +2092,6 @@ void OBCameraNode::setupDiagnosticUpdater() {
   }
 }
 
-void OBCameraNode::setupPeriodicHostTimeSync() {
-  if (time_sync_period_.count() <= 0) {
-    RCLCPP_INFO(logger_, "Periodic host time sync disabled (time_sync_period <= 0)");
-    return;
-  }
-  try {
-    RCLCPP_INFO_STREAM(
-        logger_, "Enable periodic host time sync every " << time_sync_period_.count() << " ms");
-    sync_timer_ = node_->create_wall_timer(std::chrono::milliseconds(time_sync_period_), [this]() {
-      try {
-        device_->timerSyncWithHost();
-        RCLCPP_DEBUG(logger_, "Camera time synchronized with host");
-      } catch (const ob::Error &e) {
-        RCLCPP_WARN_STREAM(logger_, "Time sync failed: " << e.getMessage());
-      } catch (const std::exception &e) {
-        RCLCPP_WARN_STREAM(logger_, "Time sync failed: " << e.what());
-      } catch (...) {
-        RCLCPP_WARN(logger_, "Time sync failed due to unknown error");
-      }
-    });
-  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to setup periodic host time sync: " << e.getMessage());
-  } catch (const std::exception &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to setup periodic host time sync: " << e.what());
-  } catch (...) {
-    RCLCPP_ERROR(logger_, "Failed to setup periodic host time sync due to unknown error");
-  }
-}
-
 void OBCameraNode::setupPipelineConfig() {
   if (pipeline_config_) {
     pipeline_config_.reset();
@@ -2315,9 +2295,6 @@ void OBCameraNode::setupPublishers() {
   std_msgs::msg::String msg;
   msg.data = filter_status_.dump(2);
   filter_status_pub_->publish(msg);
-
-  device_status_pub_ = node_->create_publisher<orbbec_camera_msgs::msg::DeviceStatus>(
-      "device_status", extrinsics_qos);
 }
 
 void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -94,6 +94,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
                    "/config/OrbbecSDKConfig_v2.0.xml"),
       logger_(this->get_logger()),
       extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
+  node_name_ = "orbbec_camera_node";
   init();
 }
 
@@ -105,6 +106,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
                    "/config/OrbbecSDKConfig_v2.0.xml"),
       logger_(this->get_logger()),
       extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
+  node_name_ = node_name;
   init();
 }
 
@@ -123,13 +125,30 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
 
   // Stop timers that might access the device
   if (sync_host_time_timer_) {
-    sync_host_time_timer_->cancel();
+    try {
-    sync_host_time_timer_.reset();
+      sync_host_time_timer_->cancel();
+      sync_host_time_timer_.reset();
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup in destructor");
+    }
   }
 
   if (check_connect_timer_) {
-    check_connect_timer_->cancel();
+    try {
-    check_connect_timer_.reset();
+      check_connect_timer_->cancel();
+      check_connect_timer_.reset();
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Exception during check connect timer cleanup in destructor");
+    }
+  }
+
+  if (device_status_timer_) {
+    try {
+      device_status_timer_->cancel();
+      device_status_timer_.reset();
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Exception during device status timer cleanup in destructor");
+    }
   }
 
   // Now stop threads
@@ -171,6 +190,8 @@ void OBCameraNodeDriver::init() {
   auto log_level = obLogSeverityFromString(log_level_str);
   connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
   enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
+  double time_sync_period = declare_parameter<double>("time_sync_period", 60.0);
+  time_sync_period_ = std::chrono::milliseconds((int)(time_sync_period * 1000));
   upgrade_firmware_ = declare_parameter<std::string>("upgrade_firmware", "");
   g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
   g_time_domain = declare_parameter<std::string>("time_domain", g_time_domain);
@@ -235,6 +256,10 @@ void OBCameraNodeDriver::init() {
   device_status_timer_ =
       this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz),
                               [this]() { deviceStatusTimer(); });
+
+  // Initialize device status publisher
+  device_status_pub_ = this->create_publisher<orbbec_camera_msgs::msg::DeviceStatus>(
+      "device_status", rclcpp::QoS(1).transient_local());
 }
 
 void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
@@ -388,6 +413,16 @@ void OBCameraNodeDriver::resetDevice() {
         continue;
       }
 
+      // Stop sync timer to prevent it from accessing the device during reset
+      if (sync_host_time_timer_) {
+        try {
+          sync_host_time_timer_->cancel();
+          sync_host_time_timer_.reset();
+        } catch (...) {
+          RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup during reset");
+        }
+      }
+
       RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
       std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
       {
@@ -448,30 +483,165 @@ void OBCameraNodeDriver::resetDevice() {
 }
 
 void OBCameraNodeDriver::deviceStatusTimer() {
-  if (ob_camera_node_ == nullptr) {
+  // Always publish device status regardless of device connection state
-    return;
-  }
   orbbec_camera_msgs::msg::DeviceStatus status_msg;
   status_msg.header.stamp = this->now();
   status_msg.device_online = device_connected_.load();
+  status_msg.header.frame_id = node_name_;
 
-  ob_camera_node_->getColorStatus(status_msg);
+  // Initialize default values for when device is not connected
-  ob_camera_node_->getDepthStatus(status_msg);
+  status_msg.connection_type = "";
+  status_msg.calibration_from_factory = false;
+  status_msg.calibration_from_launch_param = false;
+  status_msg.customer_calibration_ready = false;
 
-  status_msg.connection_type = device_info_->getConnectionType();
+  // Flag to track if device communication error occurs
+  bool device_communication_error = false;
 
-  auto camera_params = device_->getCalibrationCameraParamList();
+  // Only try to get device information if device is connected and stable
-  bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
+  if (device_connected_.load() && !device_connecting_.load()) {
-  status_msg.calibration_from_factory = calibration_from_factory;
+    // Check if reset is in progress
-  status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
+    std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_,
+                                                               std::try_to_lock);
+    if (reset_lock.owns_lock() && !reset_device_flag_) {
+      // Only get device-specific info if we have a valid camera node and device
+      if (ob_camera_node_) {
+        // Safely get color and depth status - these may access device
+        try {
+          ob_camera_node_->getColorStatus(status_msg);
+          ob_camera_node_->getDepthStatus(status_msg);
+        } catch (const ob::Error &e) {
+          std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
+          if (error_msg.find("Device is deactivated") != std::string::npos ||
+              error_msg.find("disconnected") != std::string::npos ||
+              error_msg.find("Send control transfer failed") != std::string::npos) {
+            RCLCPP_WARN(
+                logger_,
+                "Device communication error in %s at line %d: %s - Device may be disconnected",
+                __FUNCTION__, __LINE__, error_msg.c_str());
+            device_communication_error = true;
+          } else {
+            RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
+                         error_msg.c_str());
+          }
+        } catch (const std::exception &e) {
+          RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
+        } catch (...) {
+          RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
+        }
 
-  if (!ob_camera_node_->checkUserCalibrationReady()) {
+        // These should be safe as they don't directly access hardware
-    status_msg.customer_calibration_ready = false;
+        status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
-  } else {
+      }
-    status_msg.customer_calibration_ready = true;
+
+      // Safely get connection type
+      try {
+        if (device_info_) {
+          status_msg.connection_type = device_info_->getConnectionType();
+        }
+      } catch (const ob::Error &e) {
+        std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
+        if (error_msg.find("Device is deactivated") != std::string::npos ||
+            error_msg.find("disconnected") != std::string::npos ||
+            error_msg.find("Send control transfer failed") != std::string::npos) {
+          RCLCPP_WARN(
+              logger_,
+              "Device communication error in %s at line %d: %s - Device may be disconnected",
+              __FUNCTION__, __LINE__, error_msg.c_str());
+          device_communication_error = true;
+        } else {
+          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
+                       error_msg.c_str());
+        }
+      } catch (const std::exception &e) {
+        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
+      } catch (...) {
+        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
+      }
+
+      // Safely get calibration info
+      try {
+        if (device_) {
+          auto camera_params = device_->getCalibrationCameraParamList();
+          bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
+          status_msg.calibration_from_factory = calibration_from_factory;
+        }
+      } catch (const ob::Error &e) {
+        std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
+        if (error_msg.find("Device is deactivated") != std::string::npos ||
+            error_msg.find("disconnected") != std::string::npos ||
+            error_msg.find("Send control transfer failed") != std::string::npos) {
+          RCLCPP_WARN(
+              logger_,
+              "Device communication error in %s at line %d: %s - Device may be disconnected",
+              __FUNCTION__, __LINE__, error_msg.c_str());
+          device_communication_error = true;
+        } else {
+          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
+                       error_msg.c_str());
+        }
+      } catch (const std::exception &e) {
+        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
+      } catch (...) {
+        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
+      }
+
+      // Safely check user calibration readiness - this may also access device
+      // Only execute for 435LE devices (check by device name for network devices)
+      try {
+        if (device_info_ && ob_camera_node_) {
+          std::string device_name = device_info_->getName();
+          if (device_name.find("435Le") != std::string::npos ||
+              device_name.find("435LE") != std::string::npos) {
+            if (!ob_camera_node_->checkUserCalibrationReady()) {
+              status_msg.customer_calibration_ready = false;
+            } else {
+              status_msg.customer_calibration_ready = true;
+            }
+          } else {
+            // For non-435LE devices, set a default value or skip this check
+            status_msg.customer_calibration_ready = false;
+          }
+        } else {
+          status_msg.customer_calibration_ready = false;
+        }
+      } catch (const ob::Error &e) {
+        std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
+        if (error_msg.find("Device is deactivated") != std::string::npos ||
+            error_msg.find("disconnected") != std::string::npos ||
+            error_msg.find("Send control transfer failed") != std::string::npos) {
+          RCLCPP_WARN(
+              logger_,
+              "Device communication error in %s at line %d: %s - Device may be disconnected",
+              __FUNCTION__, __LINE__, error_msg.c_str());
+          device_communication_error = true;
+        } else {
+          RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
+                       error_msg.c_str());
+        }
+      } catch (const std::exception &e) {
+        RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
+      } catch (...) {
+        RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
+      }
+    }
   }
 
-  ob_camera_node_->publishDeviceStatus(status_msg);
+  // If device communication error occurred, set device_online to false
+  if (device_communication_error) {
+    status_msg.device_online = false;
+  }
+
+  // if status_msg.connection_type is empty, set it to "unknown"
+  if (status_msg.connection_type.empty()) {
+    status_msg.connection_type = "unknown";
+    status_msg.device_online = false;
+  }
+
+  // Always publish the status message, regardless of device state
+  if (device_status_pub_) {
+    device_status_pub_->publish(status_msg);
+  }
   // RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
 }
 
@@ -705,21 +875,68 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) {
     TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
     if (g_time_domain != "global") {
-      sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(60000), [this]() {
+      sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
-        if (device_) {
+        // Multiple safety checks before attempting time sync
-          TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
+        if (!device_) {
+          RCLCPP_DEBUG_STREAM(logger_, "sync_host_time_timer_: device is null, skip time sync");
+          return;
         }
+
+        // Check device connection status
+        if (!device_connected_.load()) {
+          RCLCPP_DEBUG_STREAM(logger_,
+                              "sync_host_time_timer_: device not connected, skip time sync");
+          return;
+        }
+
+        // Check if device is in connecting state
+        if (device_connecting_.load()) {
+          RCLCPP_DEBUG_STREAM(logger_, "sync_host_time_timer_: device connecting, skip time sync");
+          return;
+        }
+
+        // Check if reset is in progress
+        {
+          std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_,
+                                                                     std::try_to_lock);
+          if (!reset_lock.owns_lock() || reset_device_flag_) {
+            RCLCPP_DEBUG_STREAM(logger_,
+                                "sync_host_time_timer_: device reset in progress, skip time sync");
+            return;
+          }
+        }
+
+        // Additional safety check with device lock
+        std::unique_lock<decltype(device_lock_)> device_lock(device_lock_, std::try_to_lock);
+        if (!device_lock.owns_lock()) {
+          RCLCPP_DEBUG_STREAM(logger_,
+                              "sync_host_time_timer_: cannot acquire device lock, skip time sync");
+          return;
+        }
+
+        // Verify device is still valid after acquiring lock
+        if (!device_) {
+          RCLCPP_DEBUG_STREAM(
+              logger_, "sync_host_time_timer_: device became null after lock, skip time sync");
+          return;
+        }
+
+        RCLCPP_INFO_STREAM(logger_, "Sync device time with host");
+        TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
       });
     }
   }
 
-  RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");
+  // Safely log device information - these calls can throw if device disconnects
-  RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->getSerialNumber());
+  TRY_EXECUTE_BLOCK({
-  RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->getFirmwareVersion());
+    RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");
-  RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->getHardwareVersion());
+    RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->getSerialNumber());
+    RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->getFirmwareVersion());
+    RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->getHardwareVersion());
+    RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
+  });
   RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
   RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
-  RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
   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");
@@ -727,12 +944,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   if (!upgrade_firmware_.empty()) {
     firmware_update_success_ = false;
 
-    ob_camera_node_->withDeviceLock([&]() {
+    TRY_EXECUTE_BLOCK({
-      device_->updateFirmware(
+      ob_camera_node_->withDeviceLock([&]() {
-          upgrade_firmware_.c_str(),
+        device_->updateFirmware(
-          std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
+            upgrade_firmware_.c_str(),
-                    std::placeholders::_2, std::placeholders::_3),
+            std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
-          false);
+                      std::placeholders::_2, std::placeholders::_3),
+            false);
+      });
     });
     if (firmware_update_success_) {
       return;
@@ -1041,7 +1260,7 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
         sync_host_time_timer_->cancel();
         sync_host_time_timer_.reset();
       } catch (...) {
-        // Ignore exceptions during timer cleanup
+        RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup in firmware update");
       }
     }