Merge remote-tracking branch 'origin/feature/log_2.8.2' into merge/sdk_2.8.2

2026-10-03 19:47:46 +08:00 · 2026-04-15 09:25:53 +08:00
parent 6894dbeffa 953a725c3d
commit fa7e7086c9
6 changed files with 520 additions and 401 deletions
@@ -169,7 +169,12 @@ def generate_launch_description():
        DeclareLaunchArgument(
            'log_level',
            default_value='none',
-            description='SDK log level: none, debug, info, warn, error, fatal.'
+            description='Shared SDK and ROS log level: none, debug, info, warn, error, fatal.'
+        ),
+        DeclareLaunchArgument(
+            'log_file_name',
+            default_value='',
+            description='Custom log file name for SDK logs. If empty, default naming is used.'
        ),
        DeclareLaunchArgument(
            'time_domain',
@@ -27,6 +27,7 @@ def generate_launch_arguments():
        #general config
        DeclareLaunchArgument('camera_model', default_value=default_camera_model),
        DeclareLaunchArgument('config_file_path', default_value=''),
+        DeclareLaunchArgument('log_level', default_value='none'),
        #multi-device sync param
        DeclareLaunchArgument('camera_name', default_value='camera'),
        DeclareLaunchArgument('usb_port', default_value=''),
@@ -182,4 +183,4 @@ def generate_launch_description():
        args + [
            OpaqueFunction(function=lambda context: create_node_action(context, args))
        ]
-    )
+    )
@@ -22,6 +22,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <ament_index_cpp/get_package_prefix.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
+#include <rcutils/logging.h>
 #include <csignal>
 #include <sys/mman.h>
 #include <unistd.h>
@@ -96,6 +97,24 @@ void signalHandler(int sig) {

 namespace orbbec_camera {
 backward::SignalHandling OBCameraNodeDriver::sh;
+
+namespace {
+int rosLogSeverityFromString(const std::string_view &log_level) {
+  if (log_level == "debug") {
+    return RCUTILS_LOG_SEVERITY_DEBUG;
+  } else if (log_level == "info") {
+    return RCUTILS_LOG_SEVERITY_INFO;
+  } else if (log_level == "warn") {
+    return RCUTILS_LOG_SEVERITY_WARN;
+  } else if (log_level == "error") {
+    return RCUTILS_LOG_SEVERITY_ERROR;
+  } else if (log_level == "fatal") {
+    return RCUTILS_LOG_SEVERITY_FATAL;
+  }
+  return RCUTILS_LOG_SEVERITY_UNSET;
+}
+}  // namespace
+
 OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
    : Node("orbbec_camera_node", "/", node_options),
      node_options_(node_options),
@@ -205,12 +224,19 @@ void OBCameraNodeDriver::init() {
  g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
  auto log_level_str = declare_parameter<std::string>("log_level", "none");
  auto log_level = obLogSeverityFromString(log_level_str);
+  auto ros_log_level = rosLogSeverityFromString(log_level_str);
  auto log_file_name = declare_parameter<std::string>("log_file_name", "");
  std::string pwd_dir = std::getenv("PWD") ? std::getenv("PWD") : std::getenv("HOME");
  std::string log_path = pwd_dir + "/Log/" + g_camera_name;
  // Set logger to console
  ob::Context::setLoggerToConsole(log_level);
  ob::Context::setLoggerToFile(log_level, log_path.c_str());
+  if (ros_log_level != RCUTILS_LOG_SEVERITY_UNSET) {
+    auto ret = rcutils_logging_set_logger_level(this->get_logger().get_name(), ros_log_level);
+    if (ret != RCUTILS_RET_OK) {
+      RCLCPP_WARN_STREAM(logger_, "Failed to set ROS log level to " << log_level_str);
+    }
+  }
  // Set custom log file name if specified
  if (!log_file_name.empty()) {
    try {
@@ -284,13 +310,14 @@ void OBCameraNodeDriver::init() {
                                                     << device_access_mode_ << ")");
  if (uvc_backend_ == "libuvc") {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
-    RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
+    RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
  } else if (uvc_backend_ == "v4l2") {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_V4L2);
-    RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
+    RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
  } else {
    ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
-    RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
+    RCLCPP_WARN_STREAM(logger_,
+                       "Unsupported uvc_backend '" << uvc_backend_ << "', using default libuvc");
  }
  ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
  device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -320,17 +347,16 @@ void OBCameraNodeDriver::init() {
 void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
  CHECK_NOTNULL(device_list);
  {
-    RCLCPP_INFO_STREAM(logger_, "onDeviceConnected called");
+    RCLCPP_INFO_STREAM(logger_, "Device connected callback triggered");
    std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
    if (reset_device_flag_) {
-      RCLCPP_INFO_STREAM(logger_, "onDeviceConnected : device reset in progress, waiting...");
+      RCLCPP_INFO_STREAM(logger_, "Device reset in progress, waiting before connecting");
      reset_device_cond_.wait(
          reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
      if (!is_alive_) {
        return;
      }
-      RCLCPP_INFO_STREAM(logger_,
-                         "onDeviceConnected : device reset completed, continuing connection");
+      RCLCPP_INFO_STREAM(logger_, "Device reset completed, continuing connection");
    }
  }
  if (device_list->getCount() == 0) {
@@ -378,11 +404,9 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
    RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
    if (uid == device_unique_id_ || serial_number_ == serial_number) {
      RCLCPP_INFO_STREAM(logger_,
-                         "device with " << uid << " disconnected, notify reset device thread 1.");
+                         "device with " << uid << " disconnected, notify reset device thread");
      reset_device_flag_ = true;
      reset_device_cond_.notify_all();
-      RCLCPP_INFO_STREAM(logger_,
-                         "device with " << uid << " disconnected, notify reset device thread 2.");
      break;
    }
  }
@@ -498,7 +522,7 @@ void OBCameraNodeDriver::resetDevice() {
        }
      }

-      RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
+      RCLCPP_INFO_STREAM(logger_, "Resetting device UID: " << device_unique_id_);
      std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
      {
        // Mark device as disconnected immediately to prevent other threads from accessing it
@@ -517,7 +541,7 @@ void OBCameraNodeDriver::resetDevice() {

        if (device_) {
          try {
-            RCLCPP_INFO_STREAM(logger_, "Resetting device_");
+            RCLCPP_INFO_STREAM(logger_, "Resetting device handle");
            // Force free any idle memory before device reset
            if (ctx_) {
              try {
@@ -527,7 +551,7 @@ void OBCameraNodeDriver::resetDevice() {
              }
            }
            device_.reset();
-            RCLCPP_INFO_STREAM(logger_, "device_ reset completed");
+            RCLCPP_INFO_STREAM(logger_, "Device handle reset complete");
          } catch (const ob::Error &e) {
            RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
                                            << orbbec_camera::formatObErrorWithStatus(e));
@@ -540,9 +564,9 @@ void OBCameraNodeDriver::resetDevice() {

        if (device_info_) {
          try {
-            RCLCPP_INFO_STREAM(logger_, "Resetting device_info_");
+            RCLCPP_INFO_STREAM(logger_, "Resetting device info");
            device_info_.reset();
-            RCLCPP_INFO_STREAM(logger_, "device_info_ reset completed");
+            RCLCPP_INFO_STREAM(logger_, "Device info reset complete");
          } catch (...) {
            RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
          }
@@ -555,7 +579,7 @@ void OBCameraNodeDriver::resetDevice() {
    }
    reset_device_cond_.notify_all();
    malloc_trim(0);
-    RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
+    RCLCPP_INFO_STREAM(logger_, "Device reset complete");
  }
 }

@@ -813,11 +837,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
  std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
                 [](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
  for (size_t i = 0; i < list->getCount(); i++) {
-    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                "Before lock: Select device serial number: " << serial_number);
+    RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
+                                 "Selecting device by serial number: " << serial_number);
    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                "After lock: Select device serial number: " << serial_number);
    try {
      auto pid = list->getPid(i);
      if (isOpenNIDevice(pid)) {
@@ -827,15 +849,16 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
        if (device_info->getSerialNumber() == serial_number) {
          RCLCPP_INFO_STREAM_THROTTLE(
              logger_, *get_clock(), 5000,
-              "Device serial number " << device_info->getSerialNumber() << " matched");
+              "Matched device serial number: " << device_info->getSerialNumber());
          return device;
        }
      } else {
        std::string sn = list->getSerialNumber(i);
-        RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Device serial number: " << sn);
+        RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
+                                     "Checking device serial number: " << sn);
        if (sn == serial_number) {
          RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                      "Device serial number " << sn << " matched");
+                                      "Matched device serial number: " << sn);
          return list->getDevice(i, device_access_mode_);
        }
      }
@@ -852,15 +875,12 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
  }
  return nullptr;
 }
-
 std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
    const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
  try {
    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                "Before lock: Select device usb port: " << usb_port);
+                                "Selecting device by USB port: " << usb_port);
    std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-    RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                "After lock: Select device usb port: " << usb_port);
    auto device = list->getDeviceByUid(usb_port.c_str(), device_access_mode_);
    if (device) {
      RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
@@ -889,11 +909,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(

 std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
    const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                              "Before lock: Select device net ip: " << net_ip);
+  RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
+                               "Selecting device by network IP: " << net_ip);
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-  RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                              "After lock: Select device net ip: " << net_ip);
  std::shared_ptr<ob::Device> device = nullptr;
  for (size_t i = 0; i < list->getCount(); i++) {
    try {
@@ -903,8 +921,8 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
      if (list->getIpAddress(i) == nullptr) {
        continue;
      }
-      RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
-                                  "FindDeviceByNetIP device net ip " << list->getIpAddress(i));
+      RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
+                                   "FindDeviceByNetIP device net ip " << list->getIpAddress(i));
      if (std::string(list->getIpAddress(i)) == net_ip) {
        RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
                                    "getDeviceByNetIP device net ip " << net_ip << " done");
@@ -942,7 +960,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
  constexpr int max_retries = 3;
  bool initialized = false;
  device_info_ = device_->getDeviceInfo();
-  RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
+  RCLCPP_DEBUG_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());

  while (retry_count < max_retries && !initialized) {
    try {
@@ -1068,14 +1086,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
  RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
  auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::high_resolution_clock::now() - start_time_);
-  RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
+  RCLCPP_DEBUG_STREAM(logger_, "Start device cost: " << time_cost.count() << " ms");

  if (!upgrade_firmware_.empty()) {
    // Check if this is a second update (reupdate scenario)
    bool is_second_update = is_reupdating_.load();

    if (is_second_update) {
-      RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update...");
+      RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update");
    } else {
      RCLCPP_INFO(logger_, "Starting firmware update from file: %s", upgrade_firmware_.c_str());
    }
@@ -1103,7 +1121,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev

    if (need_reupdate_) {
      // Some devices require a second update after reboot
-      RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically.");
+      RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically");
      // Set flag to indicate we're waiting for device to reboot for second update
      is_reupdating_ = true;
      // Keep upgrade_firmware_ path and wait for device to reconnect
@@ -1113,10 +1131,10 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev

    if (firmware_update_success_) {
      if (is_second_update) {
-        RCLCPP_INFO(logger_, "Second firmware update completed successfully!");
+        RCLCPP_INFO(logger_, "Second firmware update completed successfully");
        is_reupdating_ = false;
      } else {
-        RCLCPP_INFO(logger_, "Firmware update completed successfully!");
+        RCLCPP_INFO(logger_, "Firmware update completed successfully");
      }
      return;
    }
@@ -1129,7 +1147,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
    ob_lidar_node_->startStreams();
    ob_lidar_node_->startIMU();
  } else {
-    RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
+    RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
  }

 }  // namespace orbbec_camera
@@ -1289,7 +1307,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
      // success get lock,break
      break;
    } else if (try_lock_result == EBUSY) {
-      RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
+      RCLCPP_WARN_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } else {
      RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
@@ -1315,12 +1333,12 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
    }
    auto end_time = std::chrono::high_resolution_clock::now();
    auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
-    RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
+    RCLCPP_DEBUG_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
    start_time = std::chrono::high_resolution_clock::now();
    initializeDevice(device);
    end_time = std::chrono::high_resolution_clock::now();
    time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
-    RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
+    RCLCPP_INFO_STREAM(logger_, "Initialize device cost: " << time_cost.count() << " ms");

    if (firmware_update_success_) {
      firmware_update_success_ = false;
@@ -64,27 +64,26 @@ void OBLidarNode::setAndGetNodeParameter(
 OBLidarNode::~OBLidarNode() noexcept { clean(); }

 void OBLidarNode::rebootDevice() {
-  RCLCPP_WARN_STREAM(logger_, "Reboot device");
+  RCLCPP_INFO_STREAM(logger_, "Rebooting device");
  clean();
  if (device_) {
    device_->reboot();
-    RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
+    RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
  }
 }

 void OBLidarNode::clean() noexcept {
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-  RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBLidarNode");
+  RCLCPP_DEBUG_STREAM(logger_, "Destroying OBLidarNode");
  is_running_.store(false);
-  RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
+  RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
  if (tf_thread_ && tf_thread_->joinable()) {
    tf_thread_->join();
  }
-  RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
-  RCLCPP_WARN_STREAM(logger_, "stop streams");
+  RCLCPP_DEBUG_STREAM(logger_, "Stop streams");
  stopStreams();
  stopIMU();
-  RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE");
+  RCLCPP_DEBUG_STREAM(logger_, "OBLidarNode cleanup complete");
 }

 void OBLidarNode::setupTopics() {
@@ -95,7 +94,8 @@ void OBLidarNode::setupTopics() {
    setupProfiles();
    setupPublishers();
  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
+    RCLCPP_ERROR_STREAM(logger_,
+                        "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
    throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
@@ -115,9 +115,9 @@ void OBLidarNode::getParameters() {
    param_name = stream_name_[stream_index] + "_rate";
    setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
    rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
-    RCLCPP_INFO_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
-                                               << "  Input format:"
-                                               << magic_enum::enum_name(format_[stream_index]));
+    RCLCPP_DEBUG_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
+                                                << "  Input format:"
+                                                << magic_enum::enum_name(format_[stream_index]));
    param_name = stream_name_[stream_index] + "_frame_id";
    std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
    setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
@@ -181,6 +181,8 @@ void OBLidarNode::getParameters() {
 }

 void OBLidarNode::setupDevices() {
+  RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
+
  auto sensor_list = device_->getSensorList();
  for (size_t i = 0; i < sensor_list->getCount(); i++) {
    auto sensor = sensor_list->getSensor(i);
@@ -196,14 +198,13 @@ void OBLidarNode::setupDevices() {
  }
  for (const auto &[stream_index, enable] : enable_stream_) {
    if (enable && sensors_.find(stream_index) == sensors_.end()) {
-      RCLCPP_INFO_STREAM(logger_,
-                         magic_enum::enum_name(stream_index.first)
-                             << "sensor isn't supported by current device! -- Skipping...");
+      RCLCPP_WARN_STREAM(logger_, magic_enum::enum_name(stream_index.first)
+                                      << " sensor not supported by current device, skipping");
      enable_stream_[stream_index] = false;
    }
  }
  if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
-    RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
+    RCLCPP_INFO_STREAM(logger_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
    TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
  }
  if (!echo_mode_.empty() &&
@@ -214,9 +215,9 @@ void OBLidarNode::setupDevices() {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
    }
    RCLCPP_INFO_STREAM(
-        logger_, "Setting echo mode to "
-                     << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT) ? "First Echo"
-                                                                                  : "Last Echo"));
+        logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT)
+                                               ? "First Echo"
+                                               : "Last Echo"));
  }
  if (repetitive_scan_mode_ != -1 &&
      device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
@@ -229,7 +230,7 @@ void OBLidarNode::setupDevices() {
    } else {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
                          repetitive_scan_mode_);
-      RCLCPP_INFO_STREAM(logger_, "Setting repetitive scan mode to " << device_->getIntProperty(
+      RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
                                      OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
    }
  }
@@ -242,7 +243,7 @@ void OBLidarNode::setupDevices() {
    } else {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
-      RCLCPP_INFO_STREAM(logger_, "Setting filter level to " << device_->getIntProperty(
+      RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
                                      OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
    }
  }
@@ -255,7 +256,7 @@ void OBLidarNode::setupDevices() {
                   range.min, range.max);
    } else {
      TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
-      RCLCPP_INFO_STREAM(logger_, "Setting vertical fov to " << device_->getFloatProperty(
+      RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
                                      OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
    }
  }
@@ -303,20 +304,20 @@ void OBLidarNode::setupProfiles() {
                                                << "Format:" << format_[elem]);
        RCLCPP_INFO_STREAM(logger_, "Available profiles:");
        printSensorProfiles(sensor);
-        RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
+        RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
        exit(-1);
      }
      if (!selected_profile) {
-        RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
-                                        << " Stream: " << magic_enum::enum_name(elem.first)
-                                        << ", Stream Index: " << elem.second
-                                        << ", Scan Rate: " << rate_[elem]);
+        RCLCPP_WARN_STREAM(
+            logger_, "Requested stream configuration is not supported by the device: "
+                         << "stream=" << magic_enum::enum_name(elem.first)
+                         << ", stream_index=" << elem.second << ", scan_rate=" << rate_[elem]);
        if (default_profile) {
-          RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
-          RCLCPP_WARN_STREAM(logger_, "default scan Rate "
-                                          << magic_enum::enum_name(default_profile->getScanRate())
-                                          << "default format:"
-                                          << magic_enum::enum_name(default_profile->getFormat()));
+          RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
+          RCLCPP_WARN_STREAM(
+              logger_, "Default profile: scan_rate="
+                           << magic_enum::enum_name(default_profile->getScanRate())
+                           << ", format=" << magic_enum::enum_name(default_profile->getFormat()));
          selected_profile = default_profile;
        } else {
          RCLCPP_ERROR_STREAM(
@@ -329,11 +330,11 @@ void OBLidarNode::setupProfiles() {
      stream_profile_[elem] = selected_profile;
      rate_[elem] = selected_profile->getScanRate();
      format_[elem] = selected_profile->getFormat();
-      RCLCPP_INFO_STREAM(logger_, " stream "
-                                      << stream_name_[elem] << " is enabled - scan rate: "
-                                      << magic_enum::enum_name(selected_profile->getScanRate())
-                                      << "  format:"
-                                      << magic_enum::enum_name(selected_profile->getFormat()));
+      RCLCPP_DEBUG_STREAM(logger_, "stream "
+                                       << stream_name_[elem] << " is enabled - scan rate: "
+                                       << magic_enum::enum_name(selected_profile->getScanRate())
+                                       << "  format:"
+                                       << magic_enum::enum_name(selected_profile->getFormat()));
    }
  }
  // IMU
@@ -354,12 +355,12 @@ void OBLidarNode::setupProfiles() {
        auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
        stream_profile_[stream_index] = profile;
      }
-      RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range "
+      RCLCPP_INFO_STREAM(logger_, "Stream " << stream_name_[stream_index] << " full scale range: "
                                            << (stream_index == ACCEL ? accel_range_ : gyro_range_)
-                                            << " sample rate " << imu_rate_);
+                                            << ", sample rate: " << imu_rate_);
    } catch (const ob::Error &e) {
-      RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
-                                                        << " profile: " << orbbec_camera::formatObErrorWithStatus(e));
+      RCLCPP_INFO_STREAM(logger_, "Failed to set up " << stream_name_[stream_index] << " profile: "
+                                                      << orbbec_camera::formatObErrorWithStatus(e));
      enable_stream_[stream_index] = false;
      stream_profile_[stream_index] = nullptr;
    }
@@ -434,7 +435,8 @@ void OBLidarNode::startStreams() {
      onNewFrameSetCallback(frame_set);
    });
  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
+    RCLCPP_ERROR_STREAM(logger_,
+                        "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
    setupPipelineConfig();
    pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
      onNewFrameSetCallback(frame_set);
@@ -499,13 +501,14 @@ void OBLidarNode::startIMU() {

 void OBLidarNode::stopStreams() {
  if (!pipeline_started_ || !pipeline_) {
-    RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
+    RCLCPP_DEBUG_STREAM(logger_, "Pipeline not started or not exist, skip stop pipeline");
    return;
  }
  try {
    pipeline_->stop();
  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
+    RCLCPP_ERROR_STREAM(logger_,
+                        "Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
  }
@@ -517,14 +520,16 @@ void OBLidarNode::stopIMU() {
  }

  if (!imu_sync_output_start_ || !imuPipeline_) {
-    RCLCPP_INFO_STREAM(logger_, "IMU pipeline not started or not exist, skip stop imu pipeline");
+    RCLCPP_DEBUG_STREAM(logger_,
+                        "IMU pipeline not started or unavailable, skip stopping IMU pipeline");
    return;
  }
  try {
    imuPipeline_->stop();
    imu_sync_output_start_ = false;
  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
+    RCLCPP_ERROR_STREAM(
+        logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
  }
@@ -537,15 +542,15 @@ void OBLidarNode::setupPipelineConfig() {
  pipeline_config_ = std::make_shared<ob::Config>();
  for (const auto &stream_index : LIDAR_STREAMS) {
    if (enable_stream_[stream_index]) {
-      RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
+      RCLCPP_DEBUG_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
      auto profile = stream_profile_[stream_index]->as<ob::LiDARStreamProfile>();

      if (enable_stream_[stream_index]) {
        auto video_profile = profile;
-        RCLCPP_INFO_STREAM(logger_,
-                           "lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
-                                             << "  "
-                                             << magic_enum::enum_name(video_profile->getFormat()));
+        RCLCPP_DEBUG_STREAM(logger_,
+                            "lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
+                                              << "  "
+                                              << magic_enum::enum_name(video_profile->getFormat()));
      }

      RCLCPP_INFO_STREAM(
@@ -650,7 +655,8 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
      }
    }
  } catch (const ob::Error &e) {
-    RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
+    RCLCPP_ERROR_STREAM(
+        logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
  } catch (...) {
@@ -1244,12 +1250,13 @@ void OBLidarNode::calcAndPublishStaticTransform() {
          RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical");
        }
      } catch (const ob::Error &e) {
-        RCLCPP_WARN_STREAM(logger_,
-                           "Could not get GYRO extrinsic for verification: " << orbbec_camera::formatObErrorWithStatus(e));
+        RCLCPP_WARN_STREAM(logger_, "Could not get GYRO extrinsic for verification: "
+                                        << orbbec_camera::formatObErrorWithStatus(e));
      }

    } catch (const ob::Error &e) {
-      RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: " << orbbec_camera::formatObErrorWithStatus(e));
+      RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: "
+                                      << orbbec_camera::formatObErrorWithStatus(e));
      try {
        ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
        RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
@@ -1272,11 +1279,12 @@ void OBLidarNode::calcAndPublishStaticTransform() {
    auto timestamp = node_->now();
    publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);

-    RCLCPP_INFO_STREAM(logger_, "Publishing static transform from "
-                                    << frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
-    RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
-    RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
-                                            << ", " << Q.getW());
+    RCLCPP_DEBUG_STREAM(logger_, "Publishing static transform from "
+                                     << frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
+    RCLCPP_DEBUG_STREAM(logger_,
+                        "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
+    RCLCPP_DEBUG_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
+                                             << ", " << Q.getW());
  }
 }

@@ -645,7 +645,7 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
    auto range = device_->getIntPropertyRange(prop_id);
    if (request->data < range.min || request->data > range.max) {
      response->success = false;
-      RCLCPP_INFO_STREAM(logger_, "set gain value out of range");
+      RCLCPP_WARN_STREAM(logger_, "Gain value is out of range");
      response->message = "value out of range";
      return;
    }
@@ -717,9 +717,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
        device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t*>(&config),
                                   &data_size);
        RCLCPP_INFO_STREAM(
-            logger_, "set depth AE ROI : "
+            logger_, "Set depth AE ROI to "
                         << "[Left: " << config.x0_left << ", Right: " << config.x1_right
-                         << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]");
+                         << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
        break;
      case OB_STREAM_COLOR:
      case OB_STREAM_COLOR_LEFT:
@@ -753,9 +753,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
        device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t*>(&config),
                                   &data_size);
        RCLCPP_INFO_STREAM(
-            logger_, "set color AE ROI : "
+            logger_, "Set color AE ROI to "
                         << "[Left: " << config.x0_left << ", Right: " << config.x1_right
-                         << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]");
+                         << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
        break;
      default:
        RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
@@ -800,13 +800,13 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
    if (request->data < range.min || request->data > range.max) {
      response->success = false;
-      RCLCPP_INFO_STREAM(logger_, "set white balance value out of range");
+      RCLCPP_WARN_STREAM(logger_, "White balance value is out of range");
      response->message = "value out of range";
      return;
    }
    bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
    if (auto_white_balance) {
-      RCLCPP_WARN(logger_, "auto white balance is enabled, set white balance will be ignored");
+      RCLCPP_WARN(logger_, "Auto white balance is enabled, set white balance will be ignored");
      response->success = false;
      response->message = "auto white balance is enabled";
      return;
@@ -887,7 +887,7 @@ void OBCameraNode::setAutoExposureCallback(
    auto range = device_->getIntPropertyRange(prop_id);
    if (request->data < range.min || request->data > range.max) {
      response->success = false;
-      RCLCPP_INFO_STREAM(logger_, "set auto exposure value out of range");
+      RCLCPP_WARN_STREAM(logger_, "Auto exposure value is out of range");
      response->message = "value out of range";
      return;
    }
@@ -1274,7 +1274,7 @@ void OBCameraNode::setPtpConfigCallback(
    if (!device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL,
                                      OB_PERMISSION_READ_WRITE)) {
      response->success = false;
-      RCLCPP_ERROR(logger_, "OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL not supported or not writable");
+      RCLCPP_ERROR(logger_, "PTP clock sync property is not supported or not writable");
      return;
    }
    device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, request->data);
@@ -1333,18 +1333,19 @@ void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>&
  std::string msg;
  if (request->data) {
    if (enable_stream_[stream_index]) {
-      msg = stream_name_[stream_index] + " Already ON";
+      msg = stream_name_[stream_index] + " is already enabled";
    }
-    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " ON");
+    RCLCPP_INFO_STREAM(logger_, "Request to set sensor " << stream_name_[stream_index] << " to ON");

  } else {
    if (!enable_stream_[stream_index]) {
-      msg = stream_name_[stream_index] + " Already OFF";
+      msg = stream_name_[stream_index] + " is already disabled";
    }
-    RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " OFF");
+    RCLCPP_INFO_STREAM(logger_,
+                       "Request to set sensor " << stream_name_[stream_index] << " to OFF");
  }
  if (!msg.empty()) {
-    RCLCPP_ERROR_STREAM(logger_, msg);
+    RCLCPP_WARN_STREAM(logger_, msg);
    response->success = true;
    response->message = msg;
    return;