chore: Add TRY_EXECUTE_BLOCK macro for error handling in OBCameraNodeDriver

orbbec_camera/include/orbbec_camera/utils.h

@@ -35,6 +35,33 @@ inline void LogFatal(const char* file, int line, const std::string& message) {
 }
 }  // namespace orbbec_camera
 
+#define TRY_EXECUTE_BLOCK(block)                                                                 \
+  try {                                                                                          \
+    block;                                                                                       \
+  } catch (const ob::Error& e) {                                                                 \
+    RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__, e.getMessage()); \
+  } 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__);         \
+  }
+
+#define TRY_TO_SET_PROPERTY(func, property, value)                                             \
+  try {                                                                                        \
+    device_->func(property, value);                                                            \
+  } catch (const ob::Error& e) {                                                               \
+    RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in "     \
+                                                  << __FUNCTION__ << " at line " << __LINE__   \
+                                                  << ": " << e.getMessage());                  \
+  } catch (const std::exception& e) {                                                          \
+    RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in "     \
+                                                  << __FUNCTION__ << " at line " << __LINE__   \
+                                                  << ": " << e.what());                        \
+  } catch (...) {                                                                              \
+    RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in "     \
+                                                  << __FUNCTION__ << " at line " << __LINE__); \
+  }
+
 // Macros for checking conditions and comparing values
 #define CHECK(condition) \
   (!(condition) ? LogFatal(__FILE__, __LINE__, "Check failed: " #condition) : (void)0)

orbbec_camera/src/ob_camera_node.cpp

@@ -138,19 +138,6 @@ void OBCameraNode::clean() noexcept {
   RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
 }
 
-#define TRY_TO_SET_PROPERTY(func, property, value)                                            \
-  try {                                                                                       \
-    device_->func(property, value);                                                           \
-  } catch (const ob::Error &e) {                                                              \
-    RCLCPP_ERROR_STREAM(                                                                      \
-        logger_, "Failed to set " << property << " to " << value << ": " << e.getMessage());  \
-  } catch (const std::exception &e) {                                                         \
-    RCLCPP_ERROR_STREAM(logger_,                                                              \
-                        "Failed to set " << property << " to " << value << ": " << e.what()); \
-  } catch (...) {                                                                             \
-    RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value);            \
-  }
-
 void OBCameraNode::setupDevices() {
   auto sensor_list = device_->getSensorList();
   for (size_t i = 0; i < sensor_list->count(); i++) {

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -15,6 +15,7 @@
  *******************************************************************************/
 
 #include "orbbec_camera/ob_camera_node_driver.h"
+#include "orbbec_camera/utils.h"
 #include <fcntl.h>
 #include <semaphore.h>
 #include <sys/shm.h>
@@ -371,7 +372,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
     ob_camera_node_.reset();
   }
   int retry_count = 0;
-  const int max_retries = 3;
+  constexpr int max_retries = 3;
   bool initialized = false;
   device_info_ = device_->getDeviceInfo();
   RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
@@ -410,10 +411,10 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   CHECK_NOTNULL(device_info_.get());
   device_unique_id_ = device_info_->uid();
   if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) {
-    device_->timerSyncWithHost();
+    TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
     sync_host_time_timer_ = this->create_wall_timer(std::chrono::milliseconds(30000), [this]() {
       if (device_) {
-        device_->timerSyncWithHost();
+        TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
       }
     });
   }