diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 957796f6..d148e520 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -258,16 +258,24 @@ void OBCameraNodeDriver::init() { check_connect_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); }); CHECK_NOTNULL(check_connect_timer_); - query_thread_ = std::make_shared([this]() { queryDevice(); }); - reset_device_thread_ = std::make_shared([this]() { resetDevice(); }); + device_status_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz), [this]() { deviceStatusTimer(); }); + // transient_local durability is incompatible with intra-process comms, use volatile + auto qos = rclcpp::QoS(1); + if (!node_options_.use_intra_process_comms()) { + qos = qos.transient_local(); + } // Initialize device status publisher device_status_pub_ = this->create_publisher( - "device_status", rclcpp::QoS(1).transient_local()); + "device_status", qos); + + // Create threads LAST to ensure proper cleanup if ROS operations throw exceptions + query_thread_ = std::make_shared([this]() { queryDevice(); }); + reset_device_thread_ = std::make_shared([this]() { resetDevice(); }); } void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr &device_list) {