i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 22:29:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add StreamConfigurationError exception handling for stream setup

Browse Source
This commit is contained in:
ob-yalian
2026-09-21 15:43:33 +08:00
parent 80f9a24778
commit d28886c9a1
4 changed files with 62 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+36 -38
View File
@@ -1011,21 +1011,24 @@ void OBCameraNode::setupDevices() {
std::string token;
std::vector<int> values;
values.reserve(4);
while (std::getline(iss, token, ',')) {
values.push_back(std::stoi(token));
try {
while (std::getline(iss, token, ',')) {
values.push_back(std::stoi(token));
}
} catch (const std::exception &e) {
throw StreamConfigurationError("Invalid preset_resolution_config '" +
preset_resolution_config_ + "': " + e.what());
}
if (values.size() >= 4) {
presetResolutionConfig.width = values[0];
presetResolutionConfig.height = values[1];
presetResolutionConfig.irDecimationFactor = values[2];
presetResolutionConfig.depthDecimationFactor = values[3];
} else {
RCLCPP_WARN_STREAM(
logger_,
"Invalid preset_resolution_config parameter. "
"Expected format: width,height,ir_decimation_factor,depth_decimation_factor");
if (values.size() < 4) {
throw StreamConfigurationError(
"Invalid preset_resolution_config '" + preset_resolution_config_ +
"'. Expected format: width,height,ir_decimation_factor,depth_decimation_factor");
}
presetResolutionConfig.width = values[0];
presetResolutionConfig.height = values[1];
presetResolutionConfig.irDecimationFactor = values[2];
presetResolutionConfig.depthDecimationFactor = values[3];
RCLCPP_INFO_STREAM(
logger_, "Set preset resolution config: "
@@ -3617,7 +3620,6 @@ void OBCameraNode::setupProfiles() {
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
std::shared_ptr<ob::VideoStreamProfile> default_profile;
try {
if (is_playback_device_) {
selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
@@ -3659,31 +3661,25 @@ void OBCameraNode::setupProfiles() {
<< ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
<< ", Format: " << magic_enum::enum_name(format_[elem]));
RCLCPP_ERROR(logger_,
"Error: The device might be connected via USB 2.0. Please verify your "
"configuration and try again. The current process will now exit.");
"The requested stream profile is invalid. Please correct the stream "
"configuration and restart the node.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
exit(-1);
throw StreamConfigurationError(
"Failed to configure the requested " + stream_name_[elem] +
" stream profile: " + orbbec_camera::formatObErrorWithStatus(ex));
}
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_,
"Requested stream configuration is not supported by the device: "
<< "stream=" << magic_enum::enum_name(elem.first)
<< ", stream_index=" << elem.second << ", width=" << width_[elem]
<< ", height=" << height_[elem] << ", fps=" << fps_[elem]
<< ", format=" << magic_enum::enum_name(format_[elem]));
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
RCLCPP_WARN_STREAM(logger_, "Default profile FPS: " << default_profile->getFps());
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(logger_, "No default profile found, disabling stream "
<< magic_enum::enum_name(elem.first));
enable_stream_[elem] = false;
continue;
}
const auto message = "Requested " + stream_name_[elem] +
" stream profile is not supported by the device: "
"width=" +
std::to_string(width_[elem]) +
", height=" + std::to_string(height_[elem]) +
", fps=" + std::to_string(fps_[elem]) +
", format=" + std::string(magic_enum::enum_name(format_[elem]));
RCLCPP_ERROR_STREAM(logger_, message);
throw StreamConfigurationError(message);
}
CHECK_NOTNULL(selected_profile);
stream_profile_[elem] = selected_profile;
@@ -3717,7 +3713,7 @@ void OBCameraNode::setupProfiles() {
std::string stream_fps_message;
if (!validate301SeriesStreamFrameRates(fps_, stream_fps_message)) {
RCLCPP_ERROR_STREAM(logger_, stream_fps_message);
throw std::runtime_error(stream_fps_message);
throw StreamConfigurationError(stream_fps_message);
}
// IMU
@@ -4705,7 +4701,7 @@ void OBCameraNode::getParameters() {
"right_color_frame_queue_max_frames", 10);
const auto validate_queue_capacity = [](const char *name, int capacity) {
if (capacity < 1) {
throw std::invalid_argument(std::string(name) + " must be greater than zero");
throw StreamConfigurationError(std::string(name) + " must be greater than zero");
}
};
validate_queue_capacity("color_frame_queue_max_frames", color_frame_queue_max_frames_);
@@ -4752,12 +4748,12 @@ void OBCameraNode::getParameters() {
if (image_qos_history_[stream_index] != "DEFAULT" &&
image_qos_history_[stream_index] != "KEEP_LAST" &&
image_qos_history_[stream_index] != "KEEP_ALL") {
throw std::invalid_argument(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL");
throw StreamConfigurationError(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL");
}
param_name = stream_name_[stream_index] + "_qos_depth";
setAndGetNodeParameter<int>(image_qos_depth_[stream_index], param_name, -1);
if (image_qos_depth_[stream_index] == 0 || image_qos_depth_[stream_index] < -1) {
throw std::invalid_argument(param_name + " must be -1 or greater than zero");
throw StreamConfigurationError(param_name + " must be -1 or greater than zero");
}
param_name = stream_name_[stream_index] + "_camera_info_qos";
setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
@@ -5197,7 +5193,7 @@ void OBCameraNode::setupTopics() {
if (enable_enhanced_depth_.load()) {
std::string message;
if (!ensureEnhancedDepthFilter(message)) {
throw std::runtime_error(message);
throw StreamConfigurationError(message);
}
}
setupCameraInfo();
@@ -5206,6 +5202,8 @@ void OBCameraNode::setupTopics() {
setupPublishers();
setupDiagnosticUpdater();
exportConfigJsonIfRequested();
} catch (const StreamConfigurationError &) {
throw;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
orbbec_camera/src/ob_camera_node_driver.cpp
+19 -1
View File
@@ -480,6 +480,10 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
return;
}
if (stream_configuration_error_.load()) {
return;
}
if (!device_) {
startDevice(device_list);
}
@@ -552,6 +556,10 @@ void OBCameraNodeDriver::checkConnectTimer() {
void OBCameraNodeDriver::queryDevice() {
while (is_alive_ && rclcpp::ok()) {
if (stream_configuration_error_.load()) {
return;
}
// Check if device reset is in progress before attempting to connect
{
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
@@ -1204,6 +1212,12 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
}
initialized = true;
} catch (const StreamConfigurationError &e) {
if (!stream_configuration_error_.exchange(true)) {
RCLCPP_ERROR_STREAM(logger_, "Invalid stream configuration; shutting down: " << e.what());
rclcpp::shutdown();
}
throw;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries
@@ -1507,6 +1521,8 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
if (!device_connected_) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize net device " << net_device_ip);
}
} catch (const StreamConfigurationError &) {
device_connected_ = false;
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Exception during net device initialization: " << e.what());
device_connected_ = false;
@@ -1517,7 +1533,7 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
}
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
if (device_connected_.load()) {
if (device_connected_.load() || stream_configuration_error_.load()) {
return;
}
@@ -1608,6 +1624,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
// // Fixing 301 series hot-swap not outputting power
// ob_camera_node_->startStreams();
// }
} catch (const StreamConfigurationError &) {
device_connected_ = false;
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e));