Refactor code for improved readability and consistency

orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp

@@ -17,8 +17,8 @@ class CameraExampleNode : public rclcpp::Node {
     get_params_client_ = this->create_client<orbbec_camera_msgs::srv::GetUserCalibParams>(
         "/camera/get_user_calib_params");
     set_streams_client_ = this->create_client<std_srvs::srv::SetBool>("/camera/set_streams_enable");
-    set_color_ae_roi_client_ = this->create_client<orbbec_camera_msgs::srv::SetArrays>(
+    set_color_ae_roi_client_ =
-        "/camera/set_color_ae_roi");
+        this->create_client<orbbec_camera_msgs::srv::SetArrays>("/camera/set_color_ae_roi");
     get_device_info_client_ =
         this->create_client<orbbec_camera_msgs::srv::GetDeviceInfo>("/camera/get_device_info");
   }

orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp

@@ -49,8 +49,8 @@ class ImageSyncNode : public rclcpp::Node {
         last_time_(0.0),
         frame_interval_(1.0 / hz),
         stop_display_thread_(false) {
-    sync_topics_ =
+    sync_topics_ = this->declare_parameter<std::vector<std::string>>("sync_topics",
-        this->declare_parameter<std::vector<std::string>>("sync_topics", std::vector<std::string>{});
+                                                                     std::vector<std::string>{});
     queue_size_ = this->declare_parameter<int>("queue_size", 10);
     sync_tolerance_ms_ =
         this->declare_parameter<double>("sync_tolerance_ms", frame_interval_ * 500.0);
@@ -70,8 +70,8 @@ class ImageSyncNode : public rclcpp::Node {
     validate_topics();
     topic_infos_ = make_topic_infos(sync_topics_);
 
-    RCLCPP_INFO(this->get_logger(), "image sync node started with %zu topic(s):",
+    RCLCPP_INFO(this->get_logger(),
-                sync_topics_.size());
+                "image sync node started with %zu topic(s):", sync_topics_.size());
     for (const auto &topic : sync_topics_) {
       RCLCPP_INFO(this->get_logger(), "  %s", topic.c_str());
     }
@@ -325,16 +325,16 @@ class ImageSyncNode : public rclcpp::Node {
     using Policy = message_filters::sync_policies::ApproximateTime<Image, Image>;
     using Sync = message_filters::Synchronizer<Policy>;
     auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr>,
+    sync->registerCallback(
-                                     this, _1, _2));
+        std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr>, this, _1, _2));
     configure_synchronizer(sync);
   }
 
   void create_synchronizer_3() {
     using Policy = message_filters::sync_policies::ApproximateTime<Image, Image, Image>;
     using Sync = message_filters::Synchronizer<Policy>;
-    auto sync =
+    auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
-        std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1], subscribers_[2]);
+                                       subscribers_[2]);
     sync->registerCallback(
         std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr>, this,
                   _1, _2, _3));
@@ -346,8 +346,8 @@ class ImageSyncNode : public rclcpp::Node {
     using Sync = message_filters::Synchronizer<Policy>;
     auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
                                        subscribers_[2], subscribers_[3]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr,
+    sync->registerCallback(std::bind(
-                                                                    ImageConstPtr, ImageConstPtr>,
+        &ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr, ImageConstPtr>,
         this, _1, _2, _3, _4));
     configure_synchronizer(sync);
   }
@@ -358,9 +358,9 @@ class ImageSyncNode : public rclcpp::Node {
     using Sync = message_filters::Synchronizer<Policy>;
     auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
                                        subscribers_[2], subscribers_[3], subscribers_[4]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr,
+    sync->registerCallback(
-                                                                    ImageConstPtr, ImageConstPtr,
+        std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr,
-                                                                    ImageConstPtr>,
+                                                ImageConstPtr, ImageConstPtr>,
                   this, _1, _2, _3, _4, _5));
     configure_synchronizer(sync);
   }
@@ -369,12 +369,12 @@ class ImageSyncNode : public rclcpp::Node {
     using Policy =
         message_filters::sync_policies::ApproximateTime<Image, Image, Image, Image, Image, Image>;
     using Sync = message_filters::Synchronizer<Policy>;
-    auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
+    auto sync =
-                                       subscribers_[2], subscribers_[3], subscribers_[4],
+        std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
-                                       subscribers_[5]);
+                               subscribers_[2], subscribers_[3], subscribers_[4], subscribers_[5]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr,
+    sync->registerCallback(
-                                                                    ImageConstPtr, ImageConstPtr,
+        std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr,
-                                                                    ImageConstPtr, ImageConstPtr>,
+                                                ImageConstPtr, ImageConstPtr, ImageConstPtr>,
                   this, _1, _2, _3, _4, _5, _6));
     configure_synchronizer(sync);
   }
@@ -386,10 +386,9 @@ class ImageSyncNode : public rclcpp::Node {
     auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
                                        subscribers_[2], subscribers_[3], subscribers_[4],
                                        subscribers_[5], subscribers_[6]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr,
+    sync->registerCallback(std::bind(
-                                                                    ImageConstPtr, ImageConstPtr,
+        &ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr, ImageConstPtr,
-                                                                    ImageConstPtr, ImageConstPtr,
+                                      ImageConstPtr, ImageConstPtr, ImageConstPtr>,
-                                                                    ImageConstPtr>,
         this, _1, _2, _3, _4, _5, _6, _7));
     configure_synchronizer(sync);
   }
@@ -401,10 +400,9 @@ class ImageSyncNode : public rclcpp::Node {
     auto sync = std::make_shared<Sync>(Policy(queue_size_), subscribers_[0], subscribers_[1],
                                        subscribers_[2], subscribers_[3], subscribers_[4],
                                        subscribers_[5], subscribers_[6], subscribers_[7]);
-    sync->registerCallback(std::bind(&ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr,
+    sync->registerCallback(std::bind(
-                                                                    ImageConstPtr, ImageConstPtr,
+        &ImageSyncNode::sync_callback<ImageConstPtr, ImageConstPtr, ImageConstPtr, ImageConstPtr,
-                                                                    ImageConstPtr, ImageConstPtr,
+                                      ImageConstPtr, ImageConstPtr, ImageConstPtr, ImageConstPtr>,
-                                                                    ImageConstPtr, ImageConstPtr>,
         this, _1, _2, _3, _4, _5, _6, _7, _8));
     configure_synchronizer(sync);
   }
@@ -486,8 +484,8 @@ class ImageSyncNode : public rclcpp::Node {
         if (i >= display_images.size()) {
           break;
         }
-        display_images[i].copyTo(canvas(cv::Rect(x_offset, y_offset, display_images[i].cols,
+        display_images[i].copyTo(
-                                                 display_images[i].rows)));
+            canvas(cv::Rect(x_offset, y_offset, display_images[i].cols, display_images[i].rows)));
         x_offset += column_widths[col] + margin;
       }
       y_offset += row_heights[row] + margin;
@@ -520,10 +518,9 @@ class ImageSyncNode : public rclcpp::Node {
     const double base_t = timestamps[0];
     for (size_t i = 0; i < timestamps.size(); ++i) {
       std::cout << topic_infos_[i].camera_name << " " << topic_infos_[i].image_type
-                << " stamp: " << std::setprecision(6) << timestamps[i]
+                << " stamp: " << std::setprecision(6) << timestamps[i] << "  Delay relative to "
-                << "  Delay relative to " << topic_infos_[0].camera_name << " "
+                << topic_infos_[0].camera_name << " " << topic_infos_[0].image_type << ": "
-                << topic_infos_[0].image_type << ": " << std::setprecision(3)
+                << std::setprecision(3) << (timestamps[i] - base_t) * 1000.0 << " ms" << std::endl;
-                << (timestamps[i] - base_t) * 1000.0 << " ms" << std::endl;
     }
 
     double cur = 0.0;

orbbec_camera/include/orbbec_camera/utils.h

@@ -77,7 +77,8 @@ inline std::string formatObErrorWithStatus(const ob::Error& e) {
   } catch (const ob::Error& e) {                                                                 \
     RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in "       \
                                                   << __FUNCTION__ << " at line " << __LINE__     \
-                                                  << ": " << orbbec_camera::formatObErrorWithStatus(e)); \
+                                                  << ": "                                        \
+                                                  << orbbec_camera::formatObErrorWithStatus(e)); \
   } catch (const std::exception& e) {                                                            \
     RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in "       \
                                                   << __FUNCTION__ << " at line " << __LINE__     \
@@ -168,8 +169,7 @@ OBLogSeverity obLogSeverityFromString(const std::string_view& log_level);
 
 std::string getObSdkLogDirectory();
 
-std::string configureObSdkLoggerForTool(const std::string& tool_name,
+std::string configureObSdkLoggerForTool(const std::string& tool_name, const std::string& log_level);
-                                        const std::string& log_level);
 
 OBFormat OBFormatFromString(const std::string& format);
 

orbbec_camera/scripts/service_benchmark_node.cpp

@@ -183,7 +183,9 @@ class SingleServiceBenchmark {
               RCLCPP_INFO(nh_->get_logger(), "Call %s %d/%d succeeded (cost: %.2f ms)",
                           service_name_.c_str(), i + 1, count_, dt);
             } else {
-              RCLCPP_WARN(nh_->get_logger(), "Call %s %d/%d (cost: %.2f ms) responded with success=false, message='%s'",
+              RCLCPP_WARN(
+                  nh_->get_logger(),
+                  "Call %s %d/%d (cost: %.2f ms) responded with success=false, message='%s'",
                   service_name_.c_str(), i + 1, count_, dt, response->message.c_str());
             }
           } else {

orbbec_camera/src/ob_lidar_node.cpp

@@ -296,9 +296,9 @@ void OBLidarNode::setupProfiles() {
         }
 
       } catch (const ob::Error &ex) {
-        RCLCPP_ERROR_STREAM(
+        RCLCPP_ERROR_STREAM(logger_, "Failed to get "
-            logger_, "Failed to get " << stream_name_[elem]
+                                         << stream_name_[elem] << "  profile: "
-                                      << "  profile: " << orbbec_camera::formatObErrorWithStatus(ex));
+                                         << orbbec_camera::formatObErrorWithStatus(ex));
         RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(elem.first)
                                                 << ", Stream Index: " << elem.second
                                                 << ", Scan Rate: " << rate_[elem]
@@ -1262,9 +1262,8 @@ void OBLidarNode::calcAndPublishStaticTransform() {
         ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
         RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
       } catch (const ob::Error &e2) {
-        RCLCPP_ERROR_STREAM(
+        RCLCPP_ERROR_STREAM(logger_, "Failed to get "
-            logger_, "Failed to get " << frame_id
+                                         << frame_id << " extrinsic from both ACCEL and GYRO: "
-                                      << " extrinsic from both ACCEL and GYRO: "
                                          << orbbec_camera::formatObErrorWithStatus(e2));
         ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
       }

orbbec_camera/tools/firmware_update_tool.cpp

@@ -44,8 +44,7 @@ void waitForFirmwareLogDrain(const rclcpp::Logger &logger) {
 }
 
 bool isSdkLogEnabled(const std::string &log_level) {
-  return orbbec_camera::obLogSeverityFromString(log_level) !=
+  return orbbec_camera::obLogSeverityFromString(log_level) != OBLogSeverity::OB_LOG_SEVERITY_OFF;
-         OBLogSeverity::OB_LOG_SEVERITY_OFF;
 }
 
 struct CliArgs {
@@ -629,8 +628,7 @@ bool updatePresetFirmware(const rclcpp::Logger &logger, const std::shared_ptr<ob
 
 FirmwareUpdateResult updateFirmware(const rclcpp::Logger &logger,
                                     const std::shared_ptr<ob::Device> &device,
-                                    const std::string &firmware_path,
+                                    const std::string &firmware_path, bool firmware_log_enabled) {
-                                    bool firmware_log_enabled) {
   FirmwareUpdateResult result;
   if (firmware_path.empty()) {
     result.success = true;

orbbec_camera/tools/ob_benchmark.cpp

@@ -177,12 +177,14 @@ class ObBenchmark : public rclcpp::Node {
     std::ofstream file;
     file.open(filename, std::ios_base::app);
     if (file.is_open()) {
-      file << "Time,CPU,Memory(MB)" << "\n";
+      file << "Time,CPU,Memory(MB)"
+           << "\n";
       for (int i = skip_number_; i < usage_count_ - 1; i++) {
-        file << current_time_[i] << "," << cpu_usage_[i] << "%" << "," << memory_usage_[i] << "\n";
+        file << current_time_[i] << "," << cpu_usage_[i] << "%"
+             << "," << memory_usage_[i] << "\n";
       }
-      float cpu_average =
+      float cpu_average = std::accumulate(cpu_usage_.begin() + skip_number_,
-          std::accumulate(cpu_usage_.begin() + skip_number_, cpu_usage_.begin() + usage_count_ - 1, 0.0f) /
+                                          cpu_usage_.begin() + usage_count_ - 1, 0.0f) /
                           (usage_count_ - (skip_number_ + 1));
       float memory_average = std::accumulate(memory_usage_.begin() + skip_number_,
                                              memory_usage_.begin() + usage_count_ - 1, 0.0f) /
@@ -272,9 +274,8 @@ void kill_process(const std::string& process_name) {
     current_time_.push_back(getCurrentTimes());
     RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
                        "CurrentTimes: " << current_time_[usage_count_]);
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "CPU Usage of "
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
-                                                              << process_name_ << ":"
+                       "CPU Usage of " << process_name_ << ":" << cpu_usage_[usage_count_] << "%");
-                                                              << cpu_usage_[usage_count_] << "%");
     RCLCPP_INFO_STREAM(
         rclcpp::get_logger("ObBenchmark"),
         "Memory Usage of " << process_name_ << ":" << memory_usage_[usage_count_] << "MB");

orbbec_camera/tools/start_benchmark.cpp

@@ -68,7 +68,6 @@ class StartBenchmark : public rclcpp::Node {
   std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
       color_point_cloud_subs_;
 
-
   std::vector<std::string> camera_name_;
   std::vector<std::string> color_topics_;
   std::vector<std::string> depth_topics_;