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

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

Browse Source
This commit is contained in:
xiexun
2025-09-02 17:09:25 +08:00
parent 757ed3c4af
commit e7c4a4acef
6 changed files with 296 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+18 -41
View File
@@ -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_)),
[this]() { diagnostic_updater_->force_update(); });
diagnostic_timer_ =
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
+253 -34
View File
@@ -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();
sync_host_time_timer_.reset();
try {
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();
check_connect_timer_.reset();
try {
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) {
return;
}
// Always publish device status regardless of device connection state
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);
ob_camera_node_->getDepthStatus(status_msg);
// Initialize default values for when device is not connected
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();
bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
status_msg.calibration_from_factory = calibration_from_factory;
status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
// Only try to get device information if device is connected and stable
if (device_connected_.load() && !device_connecting_.load()) {
// 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_) {
// 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()) {
status_msg.customer_calibration_ready = false;
} else {
status_msg.customer_calibration_ready = true;
// These should be safe as they don't directly access hardware
status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
}
// 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]() {
if (device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
// Multiple safety checks before attempting time sync
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");
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());
// Safely log device information - these calls can throw if device disconnects
TRY_EXECUTE_BLOCK({
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");
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([&]() {
device_->updateFirmware(
upgrade_firmware_.c_str(),
std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3),
false);
TRY_EXECUTE_BLOCK({
ob_camera_node_->withDeviceLock([&]() {
device_->updateFirmware(
upgrade_firmware_.c_str(),
std::bind(&OBCameraNodeDriver::firmwareUpdateCallback, this, std::placeholders::_1,
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");
}
}