diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index abe66fde..939cd807 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -136,6 +137,11 @@ #define DEVICE_PATH "/dev/camsync" namespace orbbec_camera { +class StreamConfigurationError : public std::runtime_error { + public: + explicit StreamConfigurationError(const std::string& message) : std::runtime_error(message) {} +}; + using GetDeviceConfig = orbbec_camera_msgs::srv::GetDeviceConfig; using GetActionConfig = orbbec_camera_msgs::srv::GetActionConfig; using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index a132d177..c9a986af 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -114,6 +114,7 @@ class OBCameraNodeDriver : public rclcpp::Node { std::atomic_bool is_alive_{false}; std::atomic_bool device_connected_{false}; std::atomic_bool device_connecting_{false}; + std::atomic_bool stream_configuration_error_{false}; std::string serial_number_; std::string device_unique_id_; std::string usb_port_; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index a1cd7891..8a88e81a 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -1011,21 +1011,24 @@ void OBCameraNode::setupDevices() { std::string token; std::vector 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 selected_profile; - std::shared_ptr default_profile; try { if (is_playback_device_) { selected_profile = profiles->getProfile(0)->as(); @@ -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(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(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)); diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index db77106e..1b1d6748 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -480,6 +480,10 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr 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 reset_lock(reset_device_mutex_); @@ -1204,6 +1212,12 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &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 &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 &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));