Add accel and gyro topic publish

orbbec_camera/include/orbbec_camera/ob_lidar_node.h

@@ -145,6 +145,10 @@ class OBLidarNode {
 
   void startStreams();
 
+  void startIMUSyncStream();
+
+  void startIMU();
+
  private:
   void setupDevices();
 
@@ -164,8 +168,16 @@ class OBLidarNode {
 
   void stopStreams();
 
+  void stopIMU();
+
   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);
+
   std::vector<OBLiDARPoint> spherePointToPoint(OBLiDARSpherePoint* sphere_point,
                                                uint32_t point_count);
 
@@ -183,6 +195,10 @@ class OBLidarNode {
 
   sensor_msgs::msg::PointCloud2 filterPointCloud(sensor_msgs::msg::PointCloud2& point_cloud) const;
 
+  orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
+
+  void setDefaultIMUMessage(sensor_msgs::msg::Imu& imu_msg);
+
   void publishStaticTransforms();
 
   void calcAndPublishStaticTransform();
@@ -199,6 +215,7 @@ class OBLidarNode {
   rclcpp::Logger logger_;
   std::atomic_bool is_running_{false};
   std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
+  std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
   std::atomic_bool pipeline_started_{false};
   std::string camera_name_ = "camera";
   std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
@@ -253,7 +270,22 @@ class OBLidarNode {
   double angle_increment_ = 0.0;
   bool enable_cloud_accumulated_ = false;
   int cloud_accumulation_count_ = -1;
-  std::unique_ptr<CloudAccumulated> cloud_accumulated_ = nullptr;
+
+  // 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 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";
+  double liner_accel_cov_ = 0.0001;
+  double angular_vel_cov_ = 0.0001;
+  //   std::unique_ptr<CloudAccumulated> cloud_accumulated_ = nullptr;
 };
 
 }  // namespace orbbec_lidar

orbbec_camera/launch/lidar.launch.py

@@ -62,8 +62,6 @@ def generate_launch_description():
         DeclareLaunchArgument('lidar_rate', default_value='20'),
         DeclareLaunchArgument('enable_scan_to_point', default_value='false'),
         DeclareLaunchArgument('repetitive_scan_mode', default_value='-1'),
-        DeclareLaunchArgument('enable_cloud_accumulated', default_value='false'),
-        DeclareLaunchArgument('cloud_accumulation_count', default_value='-1'),
         DeclareLaunchArgument('filter_level', default_value='-1'),
         DeclareLaunchArgument('vertical_fov', default_value='-1.0'),
         DeclareLaunchArgument('min_angle', default_value='-135.0'),
@@ -79,6 +77,14 @@ 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('enable_accel_data_correction', default_value='true'),
+        DeclareLaunchArgument('accel_rate', default_value='200hz'),
+        DeclareLaunchArgument('accel_range', default_value='4g'),
+        DeclareLaunchArgument('enable_gyro', default_value='false'),
+        DeclareLaunchArgument('gyro_rate', default_value='200hz'),
+        DeclareLaunchArgument('gyro_range', default_value='1000dps'),
     ]
 
     def get_params(context, args):

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -529,8 +529,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
     ob_camera_node_->startIMU();
     ob_camera_node_->startStreams();
   } else if (ob_lidar_node_) {
-    //   ob_lidar_node_->startIMU();
     ob_lidar_node_->startStreams();
+    ob_lidar_node_->startIMU();
   } else {
     RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
   }

orbbec_camera/src/ob_lidar_node.cpp

@@ -46,6 +46,8 @@ OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
                      "OBLidarNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
   is_running_.store(true);
   stream_name_[LIDAR] = "lidar";
+  stream_name_[ACCEL] = "accel";
+  stream_name_[GYRO] = "gyro";
   setupTopics();
 #if defined(USE_RK_HW_DECODER)
   jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
@@ -53,11 +55,11 @@ OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
   jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #endif
   is_camera_node_initialized_ = true;
-  if (enable_cloud_accumulated_ && cloud_accumulation_count_ >= 1) {
+  //   if (enable_cloud_accumulated_ && cloud_accumulation_count_ >= 1) {
-    auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
+  //     auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
-    cloud_accumulated_ = std::make_unique<CloudAccumulated>(node_, point_cloud_qos_profile,
+  //     cloud_accumulated_ = std::make_unique<CloudAccumulated>(node_, point_cloud_qos_profile,
-                                                            cloud_accumulation_count_);
+  //                                                             cloud_accumulation_count_);
-  }
+  //   }
 }
 
 template <class T>
@@ -97,6 +99,7 @@ void OBLidarNode::clean() noexcept {
   RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
   RCLCPP_WARN_STREAM(logger_, "stop streams");
   stopStreams();
+  stopIMU();
   RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE");
 }
 
@@ -154,7 +157,31 @@ void OBLidarNode::getParameters() {
   setAndGetNodeParameter<int>(filter_level_, "filter_level", -1);
   setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
   setAndGetNodeParameter<bool>(enable_cloud_accumulated_, "enable_cloud_accumulated", false);
-  setAndGetNodeParameter<bool>(cloud_accumulation_count_, "cloud_accumulation_count", -1);
+  setAndGetNodeParameter<int>(cloud_accumulation_count_, "cloud_accumulation_count", -1);
+  setAndGetNodeParameter<bool>(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro",
+                               false);
+  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;
+    }
+    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);
+  }
 }
 
 void OBLidarNode::setupDevices() {
@@ -164,6 +191,8 @@ void OBLidarNode::setupDevices() {
     auto profiles = sensor->getStreamProfileList();
     for (size_t j = 0; j < profiles->getCount(); j++) {
       auto profile = profiles->getProfile(j);
+      RCLCPP_INFO_STREAM(logger_,
+                         "profile->getType():" << magic_enum::enum_name((profile->getType())));
       stream_index_pair sip{profile->getType(), 0};
       if (sensors_.find(sip) != sensors_.end()) {
         continue;
@@ -171,6 +200,14 @@ void OBLidarNode::setupDevices() {
       sensors_[sip] = sensor;
     }
   }
+  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...");
+      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"));
     TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
@@ -305,6 +342,34 @@ void OBLidarNode::setupProfiles() {
                                       << magic_enum::enum_name(selected_profile->getFormat()));
     }
   }
+  // IMU
+  for (const auto &stream_index : HID_STREAMS) {
+    if (!enable_stream_[stream_index]) {
+      continue;
+    }
+    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 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 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]);
+    } catch (const ob::Error &e) {
+      RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
+                                                        << " profile: " << e.getMessage());
+      enable_stream_[stream_index] = false;
+      stream_profile_[stream_index] = nullptr;
+    }
+  }
 }
 
 void OBLidarNode::selectBaseStream() {
@@ -343,6 +408,39 @@ 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 (use_intra_process_) {
+      data_qos = rmw_qos_profile_default;
+    }
+    imu_gyro_accel_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));
+    }
+  }
 }
 
 void OBLidarNode::startStreams() {
@@ -369,6 +467,71 @@ void OBLidarNode::startStreams() {
   pipeline_started_.store(true);
 }
 
+void OBLidarNode::startIMUSyncStream() {
+  if (imuPipeline_ != nullptr) {
+    imuPipeline_.reset();
+  }
+
+  imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
+  if (imu_sync_output_start_) {
+    return;
+  }
+
+  // ACCEL
+  auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
+  auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
+  auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
+  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 gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
+  std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
+  imuConfig->enableStream(accelProfile);
+  imuConfig->enableStream(gyroProfile);
+  TRY_EXECUTE_BLOCK(imuPipeline_->enableFrameSync());
+  imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
+    auto frameSet = frame->as<ob::FrameSet>();
+    auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
+    auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
+    if (aFrame && gFrame) {
+      onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
+    }
+  });
+
+  imu_sync_output_start_ = true;
+  if (!imu_sync_output_start_) {
+    RCLCPP_ERROR_STREAM(
+        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);
+            });
+      }
+    }
+  }
+}
+
 void OBLidarNode::stopStreams() {
   if (!pipeline_started_ || !pipeline_) {
     RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
@@ -383,6 +546,36 @@ 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;
+      }
+    }
+  }
+}
+
 void OBLidarNode::setupPipelineConfig() {
   if (pipeline_config_) {
     pipeline_config_.reset();
@@ -410,6 +603,100 @@ void OBLidarNode::setupPipelineConfig() {
   }
 }
 
+void OBLidarNode::onNewIMUFrameSyncOutputCallback(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");
+    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;
+  if (!has_subscriber) {
+    return;
+  }
+  auto imu_msg = sensor_msgs::msg::Imu();
+  setDefaultIMUMessage(imu_msg);
+
+  imu_msg.header.frame_id = optical_frame_id_[GYRO];
+  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;
+  imu_msg.angular_velocity.y = gyroData.y;
+  imu_msg.angular_velocity.z = gyroData.z;
+
+  auto accel_frame = accelframe->as<ob::AccelFrame>();
+  auto accelData = accel_frame->getValue();
+  imu_msg.linear_acceleration.x = accelData.x;
+  imu_msg.linear_acceleration.y = accelData.y;
+  imu_msg.linear_acceleration.z = accelData.z;
+
+  imu_gyro_accel_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;
@@ -804,6 +1091,66 @@ void OBLidarNode::calcAndPublishStaticTransform() {
                                             << ", " << Q.getW());
   }
 }
+
+orbbec_camera_msgs::msg::IMUInfo OBLidarNode::createIMUInfo(const stream_index_pair &stream_index) {
+  orbbec_camera_msgs::msg::IMUInfo imu_info;
+  imu_info.header.frame_id = optical_frame_id_[stream_index];
+  imu_info.header.stamp = node_->now();
+
+  if (stream_index == GYRO) {
+    auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
+    auto gyro_intrinsics = gyro_profile->getIntrinsic();
+    imu_info.noise_density = gyro_intrinsics.noiseDensity;
+    imu_info.random_walk = gyro_intrinsics.randomWalk;
+    imu_info.reference_temperature = gyro_intrinsics.referenceTemp;
+    imu_info.bias = {gyro_intrinsics.bias[0], gyro_intrinsics.bias[1], gyro_intrinsics.bias[2]};
+    imu_info.scale_misalignment = {
+        gyro_intrinsics.scaleMisalignment[0], gyro_intrinsics.scaleMisalignment[1],
+        gyro_intrinsics.scaleMisalignment[2], gyro_intrinsics.scaleMisalignment[3],
+        gyro_intrinsics.scaleMisalignment[4], gyro_intrinsics.scaleMisalignment[5],
+        gyro_intrinsics.scaleMisalignment[6], gyro_intrinsics.scaleMisalignment[7],
+        gyro_intrinsics.scaleMisalignment[8]};
+    imu_info.temperature_slope = {
+        gyro_intrinsics.tempSlope[0], gyro_intrinsics.tempSlope[1], gyro_intrinsics.tempSlope[2],
+        gyro_intrinsics.tempSlope[3], gyro_intrinsics.tempSlope[4], gyro_intrinsics.tempSlope[5],
+        gyro_intrinsics.tempSlope[6], gyro_intrinsics.tempSlope[7], gyro_intrinsics.tempSlope[8]};
+  } else if (stream_index == ACCEL) {
+    auto accel_profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
+    auto accel_intrinsics = accel_profile->getIntrinsic();
+    imu_info.noise_density = accel_intrinsics.noiseDensity;
+    imu_info.random_walk = accel_intrinsics.randomWalk;
+    imu_info.reference_temperature = accel_intrinsics.referenceTemp;
+    imu_info.bias = {accel_intrinsics.bias[0], accel_intrinsics.bias[1], accel_intrinsics.bias[2]};
+    imu_info.gravity = {accel_intrinsics.gravity[0], accel_intrinsics.gravity[1],
+                        accel_intrinsics.gravity[2]};
+    imu_info.scale_misalignment = {
+        accel_intrinsics.scaleMisalignment[0], accel_intrinsics.scaleMisalignment[1],
+        accel_intrinsics.scaleMisalignment[2], accel_intrinsics.scaleMisalignment[3],
+        accel_intrinsics.scaleMisalignment[4], accel_intrinsics.scaleMisalignment[5],
+        accel_intrinsics.scaleMisalignment[6], accel_intrinsics.scaleMisalignment[7],
+        accel_intrinsics.scaleMisalignment[8]};
+    imu_info.temperature_slope = {accel_intrinsics.tempSlope[0], accel_intrinsics.tempSlope[1],
+                                  accel_intrinsics.tempSlope[2], accel_intrinsics.tempSlope[3],
+                                  accel_intrinsics.tempSlope[4], accel_intrinsics.tempSlope[5],
+                                  accel_intrinsics.tempSlope[6], accel_intrinsics.tempSlope[7],
+                                  accel_intrinsics.tempSlope[8]};
+  }
+
+  return imu_info;
+}
+void OBLidarNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
+  imu_msg.header.frame_id = "imu_link";
+  imu_msg.orientation.x = 0.0;
+  imu_msg.orientation.y = 0.0;
+  imu_msg.orientation.z = 0.0;
+  imu_msg.orientation.w = 1.0;
+
+  imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  imu_msg.linear_acceleration_covariance = {
+      liner_accel_cov_, 0.0, 0.0, 0.0, liner_accel_cov_, 0.0, 0.0, 0.0, liner_accel_cov_};
+  imu_msg.angular_velocity_covariance = {
+      angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_};
+}
 void OBLidarNode::publishStaticTF(const rclcpp::Time &t, const tf2::Vector3 &trans,
                                   const tf2::Quaternion &q, const std::string &from,
                                   const std::string &to) {