feat: add StreamConfigurationError exception handling for stream setup

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -26,6 +26,7 @@
 #include <queue>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
+#include <stdexcept>
 #include <unordered_map>
 #include <unordered_set>
 #include <utility>
@@ -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;

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_;

orbbec_camera/src/ob_camera_node.cpp

@@ -1011,21 +1011,24 @@ void OBCameraNode::setupDevices() {
     std::string token;
     std::vector<int> values;
     values.reserve(4);
-    while (std::getline(iss, token, ',')) {
+    try {
-      values.push_back(std::stoi(token));
+      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) {
+    if (values.size() < 4) {
-      presetResolutionConfig.width = values[0];
+      throw StreamConfigurationError(
-      presetResolutionConfig.height = values[1];
+          "Invalid preset_resolution_config '" + preset_resolution_config_ +
-      presetResolutionConfig.irDecimationFactor = values[2];
+          "'. Expected format: width,height,ir_decimation_factor,depth_decimation_factor");
-      presetResolutionConfig.depthDecimationFactor = values[3];
-    } else {
-      RCLCPP_WARN_STREAM(
-          logger_,
-          "Invalid preset_resolution_config parameter. "
-          "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 "
+                     "The requested stream profile is invalid. Please correct the stream "
-                     "configuration and try again. The current process will now exit.");
+                     "configuration and restart the node.");
         RCLCPP_INFO_STREAM(logger_, "Available profiles:");
         printSensorProfiles(sensor);
-        RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
+        throw StreamConfigurationError(
-        exit(-1);
+            "Failed to configure the requested " + stream_name_[elem] +
+            " stream profile: " + orbbec_camera::formatObErrorWithStatus(ex));
       }
 
       if (!selected_profile) {
-        RCLCPP_WARN_STREAM(logger_,
+        const auto message = "Requested " + stream_name_[elem] +
-                           "Requested stream configuration is not supported by the device: "
+                             " stream profile is not supported by the device: "
-                               << "stream=" << magic_enum::enum_name(elem.first)
+                             "width=" +
-                               << ", stream_index=" << elem.second << ", width=" << width_[elem]
+                             std::to_string(width_[elem]) +
-                               << ", height=" << height_[elem] << ", fps=" << fps_[elem]
+                             ", height=" + std::to_string(height_[elem]) +
-                               << ", format=" << magic_enum::enum_name(format_[elem]));
+                             ", fps=" + std::to_string(fps_[elem]) +
-        if (default_profile) {
+                             ", format=" + std::string(magic_enum::enum_name(format_[elem]));
-          RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
+        RCLCPP_ERROR_STREAM(logger_, message);
-          RCLCPP_WARN_STREAM(logger_, "Default profile FPS: " << default_profile->getFps());
+        throw StreamConfigurationError(message);
-          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;
-        }
       }
       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

@@ -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));