Merge pull request #1 from botsandus/AUTO-2610_Handle_exceptions

AUTO-2610 handle exceptions during start
botsandus · Jun 10, 2024 · a1e75db · a1e75db
2 parents c672bd7 + 8d4eab3
commit a1e75db
Showing 1 changed file with 16 additions and 7 deletions.
diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp
@@ -14,7 +14,7 @@ namespace depthai_ros_driver {
 Camera::Camera(const rclcpp::NodeOptions& options) : rclcpp::Node("camera", options) {
     ph = std::make_unique<param_handlers::CameraParamHandler>(this, "camera");
     ph->declareParams();
-    onConfigure();
+    start();
 }
 Camera::~Camera() = default;
 void Camera::onConfigure() {
@@ -71,12 +71,21 @@ void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg)
 }
 
 void Camera::start() {
-    RCLCPP_INFO(this->get_logger(), "Starting camera.");
-    if(!camRunning) {
-        onConfigure();
-    } else {
-        RCLCPP_INFO(this->get_logger(), "Camera already running!.");
-    }
+    bool success = false;
+    do {
+        try {
+            RCLCPP_INFO(this->get_logger(), "Starting camera.");
+            if(!camRunning) {
+                onConfigure();
+            } else {
+                RCLCPP_INFO(this->get_logger(), "Camera already running!.");
+            }
+            success = true;
+        } catch (const std::exception &e) {
+            RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s. Retry", e.what());
+            camRunning = false;
+        }
+    } while (!success);
 }
 
 void Camera::stop() {