Refactor: Unify accelerometer and gyroscope into single IMU control

- Replace separate accel/gyro parameters with unified IMU parameters
- Consolidate topics from separate streams to single /lidar/imu/sample
- Simplify launch file configuration with single enable_imu flag
- Update documentation and extrinsics to reflect unified structure

IMU_INTEGRATION_REPORT.md

@@ -0,0 +1,76 @@
+# IMU集成测试报告
+
+## 修改摘要
+本次修改将原来分离的加速度计和陀螺仪控制整合成统一的IMU控制，主要修改包括：
+
+### 1. Launch文件修改 (lidar.launch.py)
+**之前的参数:**
+- `enable_sync_output_accel_gyro`: 控制同步输出
+- `enable_accel`: 控制加速度计
+- `enable_gyro`: 控制陀螺仪
+- `accel_rate`: 加速度计频率
+- `gyro_rate`: 陀螺仪频率
+
+**现在的参数:**
+- `enable_imu`: 统一控制IMU(加速度计+陀螺仪)
+- `imu_rate`: 统一的IMU频率
+- `accel_range`: 加速度计量程
+- `gyro_range`: 陀螺仪量程
+
+### 2. 话题变化
+**之前的话题:**
+- `/camera/accel/sample`: 单独的加速度计数据
+- `/camera/gyro/sample`: 单独的陀螺仪数据
+- `/camera/gyro_accel/sample`: 同步的加速度计+陀螺仪数据
+
+**现在的话题:**
+- `/lidar/imu/sample`: 统一的IMU话题，包含同步的加速度计和陀螺仪数据
+
+### 3. 外部参考话题变化
+**之前的话题:**
+- `/lidar/lidar_to_accel`: LiDAR到加速度计的外部参考
+- `/lidar/lidar_to_gyro`: LiDAR到陀螺仪的外部参考
+
+**现在的话题:**
+- `/lidar/lidar_to_imu`: LiDAR到IMU的统一外部参考
+
+### 4. 测试验证
+
+#### 启动命令测试
+```bash
+# 新的启动命令
+ros2 launch orbbec_camera lidar.launch.py enable_imu:=true imu_rate:=50hz
+
+# 验证日志输出
+# 应该看到: "Started IMU stream with accel range: 2g, gyro range: 1000dps, rate: 50hz"
+```
+
+#### 话题验证
+```bash
+# 检查IMU话题
+ros2 topic list | grep -E "(imu|accel|gyro)"
+# 预期输出:
+# /lidar/imu/sample
+# /lidar/lidar_to_imu
+
+# 检查IMU数据
+ros2 topic echo /lidar/imu/sample --once
+# 预期: 包含angular_velocity和linear_acceleration的同步数据
+```
+
+### 5. 代码修改文件列表
+- `orbbec_camera/launch/lidar.launch.py`
+- `orbbec_camera/src/ob_lidar_node.cpp`
+- `orbbec_camera/include/orbbec_camera/ob_lidar_node.h`
+- `docs/launch_parameters.md`
+- `docs/all_available_topics.md`
+
+### 6. 修改优势
+1. **简化配置**: 用户只需要设置一个`enable_imu`参数即可启用整个IMU功能
+2. **统一频率**: 确保加速度计和陀螺仪使用相同的采样频率，提高数据同步性
+3. **统一话题**: 所有IMU数据在一个话题中发布，便于下游节点使用
+4. **减少复杂性**: 去除了复杂的同步选项，默认就是同步的
+5. **更好的语义**: IMU作为一个整体概念更符合机器人领域的常见用法
+
+### 7. 向后兼容性
+此修改不向后兼容，使用旧参数的launch文件需要更新参数名称。但这是一个有意的设计决策，旨在简化和改进用户体验。

docs/all_available_topics.md

@@ -17,9 +17,6 @@
 - `/camera/ir/camera_info`: The IR camera info.
 - `/camera/ir/image_raw`: The IR stream image.
 - `/camera/ir/metadata`: The IR stream firmware data.
-- `/camera/accel/sample`: Acceleration data stream `enable_sync_output_accel_gyro`turned off，`enable_accel`turned on
-- `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro `turned off，`enable_gyro`turned on
-- `camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`
-  turned on
+- `/camera/imu/sample`: Synchronized IMU data stream (accelerometer and gyroscope), when `enable_imu` is turned on
 - `/diagnostics`: The diagnostic information of the camera, Currently, the diagnostic information only includes the
   temperature of the camera.

docs/launch_parameters.md

@@ -89,26 +89,15 @@ The following are the launch parameters available:
 * **ir_ae_max_exposure**
   * Set the maximum exposure value for IR auto exposure
 * **ir_brightness**
-  * Set the IR brightness
-* **enable_sync_output_accel_gyro**
-  * Enable the sync accel_gyro,and output IMU topic real-time data
-* **enable_accel**
-  * Enable the Accelerometer,and output Accelerometer info topic data
-* **enable_accel_data_correction**
-  * Enable the Accelerometer data correction
-* **accel_rate**
-  * The frequency of the accelerometer, the optional values are `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`, `8khz`, `16khz`, `32khz`
+    * Set the IR brightness
+* **enable_imu**
+  * Enable the IMU (both accelerometer and gyroscope), and output IMU topic with synchronized data
+* **imu_rate**
+  * The frequency of the IMU, the optional values are `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`, `8khz`, `16khz`, `32khz`. The specific value depends on the current camera
 * **accel_range**
   * The range of the accelerometer, the optional values are `2g`, `4g`, `8g`, `16g`. The specific value depends on the current camera
-* **enable_gyro**
-  * Enable the gyroscope,and output gyroscope info topic data
-* **enable_gyro_data_correction**
-  * Enable the gyroscope data correction
-* **gyro_rate**
-  * The frequency of the gyroscope, the optional values are `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`, `8khz`, `16khz`, `32khz`.The specific value depends on the current camera
 * **gyro_range**
-  * The range of the gyroscope, the optional values are `16dps`, `31dps`, `62dps`, `125dps`, `250dps`, `500dps`, `1000dps`, `2000dps`. The specific value depends on the
-    current camera
+  * The range of the gyroscope, the optional values are `16dps`, `31dps`, `62dps`, `125dps`, `250dps`, `500dps`, `1000dps`, `2000dps`. The specific value depends on the current camera
 * **liner_accel_cov**
   * Covariance of the linear acceleration
 * **angular_vel_cov**

orbbec_camera/include/orbbec_camera/ob_lidar_node.h

@@ -145,8 +145,6 @@ class OBLidarNode {
 
   void startStreams();
 
-  void startIMUSyncStream();
-
   void startIMU();
 
  private:
@@ -172,11 +170,8 @@ class OBLidarNode {
 
   void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
 
-  void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
-                                       const std::shared_ptr<ob::Frame>& gryoframe);
-
-  void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
-                             const stream_index_pair& stream_index);
+  void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& accelframe,
+                             const std::shared_ptr<ob::Frame>& gyroframe);
 
   std::vector<OBLiDARPoint> spherePointToPoint(OBLiDARSpherePoint* sphere_point,
                                                uint32_t point_count);
@@ -226,9 +221,8 @@ class OBLidarNode {
   std::map<stream_index_pair, std::vector<std::shared_ptr<ob::LiDARStreamProfile>>>
       supported_profiles_;
   std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
-  std::map<stream_index_pair, OBExtrinsic> lidar_to_other_extrinsics_;
-  std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr>
-      lidar_to_other_extrinsics_publishers_;
+  OBExtrinsic lidar_to_imu_extrinsic_;
+  rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr lidar_to_imu_extrinsics_publisher_;
   stream_index_pair base_stream_ = LIDAR;
 
   std::map<stream_index_pair, bool> enable_stream_;
@@ -275,17 +269,14 @@ class OBLidarNode {
   int cloud_accumulation_count_ = -1;
 
   // IMU
-  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_gyro_accel_publisher_;
-  std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::IMUInfo>::SharedPtr>
-      imu_info_publishers_;
-  bool enable_sync_output_accel_gyro_ = false;
+  bool enable_imu_ = false;
+  std::string imu_rate_ = "50hz";
+  std::string accel_range_ = "2g";
+  std::string gyro_range_ = "1000dps";
+  std::string imu_qos_ = "default";
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
   bool imu_sync_output_start_ = false;
-  std::map<stream_index_pair, std::string> imu_rate_;
-  std::map<stream_index_pair, std::string> imu_range_;
-  std::map<stream_index_pair, std::string> imu_qos_;
-  std::map<stream_index_pair, bool> imu_started_;
-  std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame";
+  std::string accel_gyro_frame_id_ = "camera_imu_frame";
   double liner_accel_cov_ = 0.0001;
   double angular_vel_cov_ = 0.0001;
   //   std::unique_ptr<CloudAccumulated> cloud_accumulated_ = nullptr;

orbbec_camera/launch/lidar.launch.py

@@ -77,12 +77,9 @@ def generate_launch_description():
         DeclareLaunchArgument('time_domain', default_value='device'),# global, device, system
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
-        DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
-        DeclareLaunchArgument('enable_accel', default_value='false'),
-        DeclareLaunchArgument('accel_rate', default_value='50hz'),
+        DeclareLaunchArgument('enable_imu', default_value='true'),
+        DeclareLaunchArgument('imu_rate', default_value='50hz'),
         DeclareLaunchArgument('accel_range', default_value='2g'),
-        DeclareLaunchArgument('enable_gyro', default_value='false'),
-        DeclareLaunchArgument('gyro_rate', default_value='50hz'),
         DeclareLaunchArgument('gyro_range', default_value='1000dps'),
     ]
 

orbbec_camera/src/ob_lidar_node.cpp

@@ -158,29 +158,31 @@ void OBLidarNode::getParameters() {
   setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
   setAndGetNodeParameter<bool>(enable_cloud_accumulated_, "enable_cloud_accumulated", false);
   setAndGetNodeParameter<int>(cloud_accumulation_count_, "cloud_accumulation_count", -1);
-  setAndGetNodeParameter<bool>(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro",
-                               false);
+  setAndGetNodeParameter<bool>(enable_imu_, "enable_imu", false);
+  setAndGetNodeParameter<std::string>(imu_rate_, "imu_rate", "50hz");
+  setAndGetNodeParameter<std::string>(accel_range_, "accel_range", "2g");
+  setAndGetNodeParameter<std::string>(gyro_range_, "gyro_range", "1000dps");
+  setAndGetNodeParameter<std::string>(imu_qos_, "imu_qos", "default");
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
   setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
-  for (const auto &stream_index : HID_STREAMS) {
-    std::string param_name = stream_name_[stream_index] + "_qos";
-    setAndGetNodeParameter<std::string>(imu_qos_[stream_index], param_name, "default");
-    param_name = "enable_" + stream_name_[stream_index];
-    setAndGetNodeParameter<bool>(enable_stream_[stream_index], param_name, false);
-    if (enable_sync_output_accel_gyro_) {
-      enable_stream_[stream_index] = true;
+
+  // Setup IMU streams if enabled
+  if (enable_imu_) {
+    enable_stream_[ACCEL] = true;
+    enable_stream_[GYRO] = true;
+
+    for (const auto &stream_index : HID_STREAMS) {
+      std::string param_name = camera_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);
+      std::string default_optical_frame_id =
+          camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
+      param_name = stream_name_[stream_index] + "_optical_frame_id";
+      setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
     }
-    param_name = stream_name_[stream_index] + "_rate";
-    setAndGetNodeParameter<std::string>(imu_rate_[stream_index], param_name, "");
-    param_name = stream_name_[stream_index] + "_range";
-    setAndGetNodeParameter<std::string>(imu_range_[stream_index], param_name, "");
-    param_name = camera_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);
-    std::string default_optical_frame_id =
-        camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
-    param_name = stream_name_[stream_index] + "_optical_frame_id";
-    setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
+
+    // Set unified IMU frame ID
+    accel_gyro_frame_id_ = camera_name_ + "_imu_frame";
   }
 }
 
@@ -348,19 +350,19 @@ void OBLidarNode::setupProfiles() {
     try {
       auto profile_list = sensors_[stream_index]->getStreamProfileList();
       if (stream_index == ACCEL) {
-        auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
-        auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
+        auto full_scale_range = fullAccelScaleRangeFromString(accel_range_);
+        auto sample_rate = sampleRateFromString(imu_rate_);
         auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
         stream_profile_[stream_index] = profile;
       } else if (stream_index == GYRO) {
-        auto full_scale_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
-        auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
+        auto full_scale_range = fullGyroScaleRangeFromString(gyro_range_);
+        auto sample_rate = sampleRateFromString(imu_rate_);
         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 "
-                                            << imu_range_[stream_index] << " sample rate "
-                                            << imu_rate_[stream_index]);
+                                            << (stream_index == ACCEL ? accel_range_ : gyro_range_) << " sample rate "
+                                            << imu_rate_);
     } catch (const ob::Error &e) {
       RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
                                                         << " profile: " << e.getMessage());
@@ -406,52 +408,23 @@ void OBLidarNode::setupPublishers() {
         "cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                                     point_cloud_qos_profile));
   }
-  if (enable_sync_output_accel_gyro_) {
-    std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
-    auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
+  if (enable_imu_) {
+    std::string topic_name = "imu/sample";
+    auto data_qos = getRMWQosProfileFromString(imu_qos_);
     if (use_intra_process_) {
       data_qos = rmw_qos_profile_default;
     }
-    imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
+    imu_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
         topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
-    // topic_name = stream_name_[GYRO] + "/imu_info";
-    // imu_info_publishers_[GYRO] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
-    //     topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
-    // topic_name = stream_name_[ACCEL] + "/imu_info";
-    // imu_info_publishers_[ACCEL] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
-    //     topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
-  } else {
-    for (const auto &stream_index : HID_STREAMS) {
-      if (!enable_stream_[stream_index]) {
-        continue;
-      }
-      std::string data_topic_name = stream_name_[stream_index] + "/sample";
-      auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
-      if (use_intra_process_) {
-        data_qos = rmw_qos_profile_default;
-      }
-      imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
-          data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
-      //   data_topic_name = stream_name_[stream_index] + "/imu_info";
-      //   imu_info_publishers_[stream_index] =
-      //       node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
-      //           data_topic_name,
-      //           rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
-    }
   }
   auto extrinsics_qos = rclcpp::QoS(1).transient_local();
   if (use_intra_process_) {
     extrinsics_qos = rclcpp::QoS(1);
   }
-  if (enable_stream_[LIDAR] && enable_stream_[ACCEL]) {
-    lidar_to_other_extrinsics_publishers_[ACCEL] =
+  if (enable_imu_) {
+    lidar_to_imu_extrinsics_publisher_ =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/lidar_to_accel", extrinsics_qos);
-  }
-  if (enable_stream_[LIDAR] && enable_stream_[GYRO]) {
-    lidar_to_other_extrinsics_publishers_[GYRO] =
-        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/lidar_to_gyro", extrinsics_qos);
+            "/" + camera_name_ + "/lidar_to_imu", extrinsics_qos);
   }
 }
 
@@ -479,7 +452,12 @@ void OBLidarNode::startStreams() {
   pipeline_started_.store(true);
 }
 
-void OBLidarNode::startIMUSyncStream() {
+
+void OBLidarNode::startIMU() {
+  if (!enable_imu_) {
+    return;
+  }
+
   if (imuPipeline_ != nullptr) {
     imuPipeline_.reset();
   }
@@ -491,14 +469,16 @@ void OBLidarNode::startIMUSyncStream() {
 
   // ACCEL
   auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
-  auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
-  auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
+  auto accel_range = fullAccelScaleRangeFromString(accel_range_);
+  auto accel_rate = sampleRateFromString(imu_rate_);
   auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
+
   // GYRO
   auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
-  auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
-  auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
+  auto gyro_range = fullGyroScaleRangeFromString(gyro_range_);
+  auto gyro_rate = sampleRateFromString(imu_rate_);
   auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
+
   std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
   imuConfig->enableStream(accelProfile);
   imuConfig->enableStream(gyroProfile);
@@ -508,7 +488,7 @@ void OBLidarNode::startIMUSyncStream() {
     auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
     auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
     if (aFrame && gFrame) {
-      onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
+      onNewIMUFrameCallback(aFrame, gFrame);
     }
   });
 
@@ -518,29 +498,9 @@ void OBLidarNode::startIMUSyncStream() {
         logger_, "Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
   } else {
     RCLCPP_INFO_STREAM(
-        logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
-                                                   << ",rate:" << sampleRateToString(accel_rate)
-                                                   << ", and start gyro stream with range:"
-                                                   << fullGyroScaleRangeToString(gyro_range)
-                                                   << ",rate:" << sampleRateToString(gyro_rate));
-  }
-}
-void OBLidarNode::startIMU() {
-  if (enable_sync_output_accel_gyro_) {
-    startIMUSyncStream();
-  } else {
-    for (const auto &stream_index : HID_STREAMS) {
-      if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
-        auto imu_profile = stream_profile_[stream_index];
-        CHECK_NOTNULL(imu_profile);
-        RCLCPP_INFO_STREAM(logger_, "start " << stream_name_[stream_index] << " stream");
-        CHECK_NOTNULL(sensors_[stream_index]);
-        sensors_[stream_index]->start(
-            imu_profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
-              onNewIMUFrameCallback(frame, stream_index);
-            });
-      }
-    }
+        logger_, "Started IMU stream with accel range: " << fullAccelScaleRangeToString(accel_range)
+                                                         << ", gyro range: " << fullGyroScaleRangeToString(gyro_range)
+                                                         << ", rate: " << sampleRateToString(accel_rate));
   }
 }
 
@@ -559,32 +519,21 @@ void OBLidarNode::stopStreams() {
 }
 
 void OBLidarNode::stopIMU() {
-  if (enable_sync_output_accel_gyro_) {
-    if (!imu_sync_output_start_ || !imuPipeline_) {
-      RCLCPP_INFO_STREAM(logger_, "imu pipeline not started or not exist, skip stop imu pipeline");
-      return;
-    }
-    try {
-      imuPipeline_->stop();
-    } catch (const ob::Error &e) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
-    } catch (...) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline");
-    }
-  } else {
-    for (const auto &stream_index : HID_STREAMS) {
-      if (imu_started_[stream_index]) {
-        CHECK(sensors_.count(stream_index));
-        RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
-        try {
-          sensors_[stream_index]->stop();
-        } catch (const ob::Error &e) {
-          RCLCPP_ERROR_STREAM(logger_, "Failed to stop " << stream_name_[stream_index]
-                                                         << " stream: " << e.getMessage());
-        }
-        imu_started_[stream_index] = false;
-      }
-    }
+  if (!enable_imu_) {
+    return;
+  }
+
+  if (!imu_sync_output_start_ || !imuPipeline_) {
+    RCLCPP_INFO_STREAM(logger_, "IMU pipeline not started or not exist, skip stop 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: " << e.getMessage());
+  } catch (...) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
   }
 }
 
@@ -615,40 +564,29 @@ void OBLidarNode::setupPipelineConfig() {
   }
 }
 
-void OBLidarNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
+void OBLidarNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &accelframe,
                                                   const std::shared_ptr<ob::Frame> &gryoframe) {
   if (!is_camera_node_initialized_) {
     return;
   }
-  if (!imu_gyro_accel_publisher_) {
-    RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
+  if (!imu_publisher_) {
+    RCLCPP_ERROR_STREAM(logger_, "IMU publisher not initialized");
     return;
   }
-  bool has_subscriber = imu_gyro_accel_publisher_->get_subscription_count() > 0;
-  //   has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
-  //   has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
+
+  bool has_subscriber = imu_publisher_->get_subscription_count() > 0;
   if (!has_subscriber) {
     return;
   }
+
   auto imu_msg = sensor_msgs::msg::Imu();
   setDefaultIMUMessage(imu_msg);
 
-  imu_msg.header.frame_id = optical_frame_id_[GYRO];
+  imu_msg.header.frame_id = accel_gyro_frame_id_;
   auto frame_timestamp = getFrameTimestampUs(accelframe);
   auto timestamp = fromUsToROSTime(frame_timestamp);
   imu_msg.header.stamp = timestamp;
 
-  //   auto gyro_info = createIMUInfo(GYRO);
-  //   gyro_info.header = imu_msg.header;
-  //   imu_info_publishers_[GYRO]->publish(gyro_info);
-
-  //   auto accel_info = createIMUInfo(ACCEL);
-  //   imu_msg.header.frame_id = optical_frame_id_[ACCEL];
-  //   accel_info.header = imu_msg.header;
-  //   imu_info_publishers_[ACCEL]->publish(accel_info);
-
-  imu_msg.header.frame_id = accel_gyro_frame_id_;
-
   auto gyro_frame = gryoframe->as<ob::GyroFrame>();
   auto gyroData = gyro_frame->getValue();
   imu_msg.angular_velocity.x = gyroData.x;
@@ -661,54 +599,10 @@ void OBLidarNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fram
   imu_msg.linear_acceleration.y = accelData.y;
   imu_msg.linear_acceleration.z = accelData.z;
 
-  imu_gyro_accel_publisher_->publish(imu_msg);
+  imu_publisher_->publish(imu_msg);
 }
 
-void OBLidarNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
-                                        const stream_index_pair &stream_index) {
-  if (!is_camera_node_initialized_) {
-    return;
-  }
-  if (!imu_publishers_.count(stream_index)) {
-    RCLCPP_ERROR_STREAM(logger_,
-                        "stream " << stream_name_[stream_index] << " publisher not initialized");
-    return;
-  }
-  bool has_subscriber = imu_publishers_[stream_index]->get_subscription_count() > 0;
-  //   has_subscriber =
-  //       has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
-  if (!has_subscriber) {
-    return;
-  }
-  auto imu_msg = sensor_msgs::msg::Imu();
-  setDefaultIMUMessage(imu_msg);
 
-  imu_msg.header.frame_id = optical_frame_id_[stream_index];
-  auto timestamp = fromUsToROSTime(frame->getTimeStampUs());
-  imu_msg.header.stamp = timestamp;
-
-  //   auto imu_info = createIMUInfo(stream_index);
-  //   imu_info.header = imu_msg.header;
-  //   imu_info_publishers_[stream_index]->publish(imu_info);
-
-  if (frame->getType() == OB_FRAME_GYRO) {
-    auto gyro_frame = frame->as<ob::GyroFrame>();
-    auto data = gyro_frame->getValue();
-    imu_msg.angular_velocity.x = data.x;
-    imu_msg.angular_velocity.y = data.y;
-    imu_msg.angular_velocity.z = data.z;
-  } else if (frame->getType() == OB_FRAME_ACCEL) {
-    auto accel_frame = frame->as<ob::AccelFrame>();
-    auto data = accel_frame->getValue();
-    imu_msg.linear_acceleration.x = data.x;
-    imu_msg.linear_acceleration.y = data.y;
-    imu_msg.linear_acceleration.z = data.z;
-  } else {
-    RCLCPP_ERROR(logger_, "Unsupported IMU frame type");
-    return;
-  }
-  imu_publishers_[stream_index]->publish(imu_msg);
-}
 void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
   if (!is_running_.load()) {
     return;
@@ -1109,8 +1003,8 @@ void OBLidarNode::calcAndPublishStaticTransform() {
     RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
                                             << ", " << Q.getW());
   }
-  if (enable_stream_[LIDAR] && enable_stream_[ACCEL]) {
-    static const char *frame_id = "lidar_to_accel_extrinsics";
+  if (enable_imu_) {
+    static const char *frame_id = "lidar_to_imu_extrinsics";
     OBExtrinsic ex;
     try {
       ex = base_stream_profile->getExtrinsicTo(stream_profile_[ACCEL]);
@@ -1119,25 +1013,10 @@ void OBLidarNode::calcAndPublishStaticTransform() {
                           "Failed to get " << frame_id << " extrinsic: " << e.getMessage());
       ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
     }
-    lidar_to_other_extrinsics_[ACCEL] = ex;
+    lidar_to_imu_extrinsic_ = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
-    CHECK_NOTNULL(lidar_to_other_extrinsics_publishers_[ACCEL]);
-    lidar_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
-  }
-  if (enable_stream_[LIDAR] && enable_stream_[GYRO]) {
-    static const char *frame_id = "lidar_to_gyro_extrinsics";
-    OBExtrinsic ex;
-    try {
-      ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
-    } catch (const ob::Error &e) {
-      RCLCPP_ERROR_STREAM(logger_,
-                          "Failed to get " << frame_id << " extrinsic: " << e.getMessage());
-      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
-    }
-    lidar_to_other_extrinsics_[GYRO] = ex;
-    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
-    CHECK_NOTNULL(lidar_to_other_extrinsics_publishers_[GYRO]);
-    lidar_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
+    CHECK_NOTNULL(lidar_to_imu_extrinsics_publisher_);
+    lidar_to_imu_extrinsics_publisher_->publish(ex_msg);
   }
 }