i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 05:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix onTemperatureUpdate error

Browse Source
This commit is contained in:
xiexun
2025-09-05 17:11:07 +08:00
parent 5b47280625
commit 90d54a2e6f
4 changed files with 72 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+48 -28
View File
@@ -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 (!device_ || !is_running_.load() || !is_camera_node_initialized_.load()) {
if (!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_) {
diagnostic_updater_->force_update();
// Check if we're still running and all components are valid
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
+20 -1
View File
@@ -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);