add IMU topic

orbbec_camera/CMakeLists.txt

@@ -65,6 +65,7 @@ add_library(${PROJECT_NAME} SHARED
   src/ob_camera_node.cpp
   src/ros_param_backend.cpp
   src/ros_service.cpp
+  src/synced_imu_publisher.cpp
   src/utils.cpp
   )
 
@@ -221,6 +222,32 @@ ament_target_dependencies(list_depth_work_mode_node
   ${dependencies}
   )
 
+add_executable(list_camera_profile_mode_node
+  src/list_camera_profile.cpp
+  )
+
+target_include_directories(list_camera_profile_mode_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${ORBBEC_INCLUDE_DIR}
+  ${OpenCV_INCLUDED_DIRS}
+  ${GLOG_INCLUDED_DIRS}
+  )
+
+target_link_libraries(list_camera_profile_mode_node
+  ${ORBBEC_SDK_LIBRARIES}
+  ${OpenCV_LIBS}
+  Eigen3::Eigen
+  ${GLOG_LIBRARIES}
+  -lOrbbecSDK
+  -L${ORBBEC_LIBS}
+  ${PROJECT_NAME}
+  )
+
+ament_target_dependencies(list_depth_work_mode_node
+  ${dependencies}
+  )
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -258,6 +285,7 @@ install(DIRECTORY
 install(TARGETS list_devices_node
   ob_cleanup_shm_node
   list_depth_work_mode_node
+  list_camera_profile_mode_node
   ${PROJECT_NAME}_node
   DESTINATION lib/${PROJECT_NAME}/
   )

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -20,6 +20,7 @@
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
+#include <utility>
 #include <vector>
 #include <atomic>
 #include <opencv2/opencv.hpp>
@@ -37,6 +38,7 @@
 
 #include <image_publisher/image_publisher.hpp>
 #include <image_transport/publisher.hpp>
+#include <sensor_msgs/msg/imu.hpp>
 #include "libobsensor/ObSensor.hpp"
 
 #include "orbbec_camera_msgs/msg/device_info.hpp"
@@ -118,6 +120,16 @@ class OBCameraNode {
   void clean();
 
  private:
+  struct IMUData {
+    IMUData() = default;
+    IMUData(stream_index_pair stream, Eigen::Vector3d data, double timestamp)
+        : stream_(std::move(stream)), data_(std::move(data)), timestamp_(timestamp) {}
+    bool isSet() const { return timestamp_ >= 0; }
+    stream_index_pair stream_{};
+    Eigen::Vector3d data_{};
+    double timestamp_ = -1;  // in nanoseconds
+  };
+
   void setupDevices();
 
   void setupProfiles();
@@ -132,8 +144,12 @@ class OBCameraNode {
 
   void startStreams();
 
+  void startIMU();
+
   void stopStreams();
 
+  void stopIMU();
+
   void setupDefaultImageFormat();
 
   void setupPublishers();
@@ -242,6 +258,18 @@ class OBCameraNode {
   void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
                        const sensor_msgs::msg::Image::SharedPtr& image_msg);
 
+  void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
+                             const stream_index_pair& stream_index);
+
+  void setDefaultIMUMessage(sensor_msgs::msg::Imu& imu_msg);
+
+  sensor_msgs::msg::Imu createUnitIMUMessage(const IMUData& accel_data, const IMUData& gyro_data);
+
+  void FillImuDataLinearInterpolation(const IMUData& imu_data,
+                                      std::deque<sensor_msgs::msg::Imu>& imu_msgs);
+
+  void FillImuDataCopy(const IMUData& imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
+
   bool setupFormatConvertType(OBFormat format);
 
  private:
@@ -252,6 +280,7 @@ class OBCameraNode {
   std::atomic_bool is_running_{false};
   std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
   std::atomic_bool pipeline_started_{false};
+  std::string camera_name_ = "camera";
   std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
   std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
   std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
@@ -312,8 +341,8 @@ class OBCameraNode {
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
   std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr;
   std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr colored_point_cloud_publisher_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_registration_cloud_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_cloud_pub_;
   bool enable_point_cloud_ = true;
   bool enable_colored_point_cloud_ = false;
   ob::PointCloudFilter point_cloud_filter_;
@@ -355,5 +384,15 @@ class OBCameraNode {
   int device_trigger_signal_out_delay_ = 0;
   std::string depth_precision_str_;
   OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
+  // IMU
+  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
+  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_;
+  double liner_accel_cov_ = 0.0001;
+  double angular_vel_cov_ = 0.0001;
+  std::deque<IMUData> imu_history_;
+  IMUData accel_data_{ACCEL, {0, 0, 0}, -1.0};
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/synced_imu_publisher.h

@@ -0,0 +1,33 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+#include <glog/logging.h>
+#include <sensor_msgs/msg/imu.hpp>
+#include <queue>
+#include <mutex>
+#include <condition_variable>
+
+namespace orbbec_camera {
+class SyncedImuPublisher {
+ public:
+  SyncedImuPublisher(rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher,
+                     size_t queue_size = 1000);
+  ~SyncedImuPublisher();
+  void publish(const sensor_msgs::msg::Imu& imu_msg);
+  void pause();
+  void resume();
+  void setQueueSize(size_t queue_size);
+
+  void enable(bool enable);
+
+ private:
+  void publishPendingMessages();
+
+ private:
+  std::mutex mutex_;
+  std::queue<sensor_msgs::msg::Imu> queue_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+  bool is_enabled_ = true;
+  bool is_paused_ = false;
+  size_t queue_size_ = 1000;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/utils.h

@@ -55,4 +55,10 @@ OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
 
 OBSyncMode OBSyncModeFromString(const std::string& mode);
 
+OB_SAMPLE_RATE sampleRateFromString(std::string& sample_rate);
+
+OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string& full_scale_range);
+
+OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string& full_scale_range);
+
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini2.launch.xml

@@ -37,6 +37,14 @@
     <arg name="ir_qos" default="default"/>
     <arg name="ir_camera_info_qos" default="default"/>
     <arg name="enable_ir_auto_exposure" default="true"/>
+    <arg name="enable_accel" default="true"/>
+    <arg name="accel_rate" default="100hz"/>
+    <arg name="accel_range" default="4g"/>
+    <arg name="enable_gyro" default="true"/>
+    <arg name="gyro_rate" default="100hz"/>
+    <arg name="gyro_range" default="1000dps"/>
+    <arg name="liner_accel_cov" default="0.01"/>
+    <arg name="angular_vel_cov" default="0.01"/>
     <arg name="publish_tf" default="true"/>
     <arg name="tf_publish_rate" default="10.0"/>
     <arg name="ir_info_url" default=""/>
@@ -92,6 +100,14 @@
             <param name="ir_qos" value="$(var ir_qos)"/>
             <param name="ir_camera_info_qos" value="$(var ir_camera_info_qos)"/>
             <param name="enable_ir_auto_exposure" value="$(var enable_ir_auto_exposure)"/>
+            <param name="enable_accel" value="$(var enable_accel)" />
+            <param name="accel_rate" value="$(var accel_rate)" />
+            <param name="accel_range" value="$(var accel_range)" />
+            <param name="enable_gyro" value="$(var enable_gyro)" />
+            <param name="gyro_rate" value="$(var gyro_rate)" />
+            <param name="gyro_range" value="$(var gyro_range)" />
+            <param name="liner_accel_cov" value="$(var liner_accel_cov)"/>
+            <param name="angular_vel_cov" value="$(var angular_vel_cov)"/>
             <param name="publish_tf" value="$(var publish_tf)"/>
             <param name="tf_publish_rate" value="$(var tf_publish_rate)"/>
             <param name="ir_info_url" value="$(var ir_info_url)"/>

orbbec_camera/src/list_camera_profile.cpp

@@ -0,0 +1,56 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <orbbec_camera/ob_camera_node_driver.h>
+#include <memory>
+#include <magic_enum/magic_enum.hpp>
+int main() {
+  auto context = std::make_unique<ob::Context>();
+  context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
+  auto device_list = context->queryDeviceList();
+  auto device = device_list->getDevice(0);
+  auto sensor_list = device->getSensorList();
+  for (size_t i = 0; i < sensor_list->count(); i++) {
+    auto sensor = sensor_list->getSensor(i);
+    auto profile_list = sensor->getStreamProfileList();
+    for (size_t j = 0; j < profile_list->count(); j++) {
+      auto origin_profile = profile_list->getProfile(j);
+      if (sensor->type() == OB_SENSOR_COLOR) {
+        auto profile = origin_profile->as<ob::VideoStreamProfile>();
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "color profile: " << profile->width() << "x" << profile->height() << " "
+                                             << profile->fps() << "fps "
+                                             << magic_enum::enum_name(profile->format()));
+      } else if (sensor->type() == OB_SENSOR_DEPTH) {
+        auto profile = origin_profile->as<ob::VideoStreamProfile>();
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "depth profile: " << profile->width() << "x" << profile->height() << " "
+                                             << profile->fps() << "fps "
+                                             << magic_enum::enum_name(profile->format()));
+      } else if (sensor->type() == OB_SENSOR_IR) {
+        auto profile = origin_profile->as<ob::VideoStreamProfile>();
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "ir profile: " << profile->width() << "x" << profile->height() << " "
+                                          << profile->fps() << "fps "
+                                          << magic_enum::enum_name(profile->format()));
+      } else if (sensor->type() == OB_SENSOR_ACCEL) {
+        auto profile = origin_profile->as<ob::AccelStreamProfile>();
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "accel profile: sampleRate "
+                               << magic_enum::enum_name(profile->sampleRate())
+                               << "  full scale_range "
+                               << magic_enum::enum_name(profile->fullScaleRange()));
+      } else if (sensor->type() == OB_SENSOR_GYRO) {
+        auto profile = origin_profile->as<ob::GyroStreamProfile>();
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "gyro profile: sampleRate "
+                               << magic_enum::enum_name(profile->sampleRate())
+                               << "  full scale_range "
+                               << magic_enum::enum_name(profile->fullScaleRange()));
+      } else {
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                           "unknown profile: " << magic_enum::enum_name(sensor->type()));
+      }
+    }
+  }
+  return 0;
+}

orbbec_camera/src/ob_camera_node.cpp

@@ -30,6 +30,9 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
   stream_name_[COLOR] = "color";
   stream_name_[DEPTH] = "depth";
   stream_name_[INFRA0] = "ir";
+  stream_name_[INFRA1] = "ir2";
+  stream_name_[ACCEL] = "accel";
+  stream_name_[GYRO] = "gyro";
 
   compression_params_.push_back(cv::IMWRITE_PNG_COMPRESSION);
   compression_params_.push_back(0);
@@ -224,6 +227,55 @@ void OBCameraNode::startStreams() {
     });
   }
   pipeline_started_.store(true);
+  startIMU();
+}
+
+void OBCameraNode::startIMU() {
+  for (const auto& stream_index : HID_STREAMS) {
+    if (enable_stream_[stream_index]) {
+      CHECK(sensors_.count(stream_index));
+      auto profile_list = sensors_[stream_index]->getStreamProfileList();
+      for (size_t i = 0; i < profile_list->count(); i++) {
+        auto item = profile_list->getProfile(i);
+        if (stream_index == ACCEL) {
+          auto profile = item->as<ob::AccelStreamProfile>();
+          auto accel_rate = sampleRateFromString(imu_rate_[stream_index]);
+          auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
+          if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
+            sensors_[stream_index]->start(profile,
+                                          [this, stream_index](std::shared_ptr<ob::Frame> frame) {
+                                            onNewIMUFrameCallback(frame, stream_index);
+                                          });
+            imu_started_[stream_index] = true;
+            RCLCPP_INFO_STREAM(logger_, "start accel stream with "
+                                            << magic_enum::enum_name(accel_range) << " range and "
+                                            << magic_enum::enum_name(accel_rate) << " rate");
+          }
+        } else if (stream_index == GYRO) {
+          auto profile = item->as<ob::GyroStreamProfile>();
+          auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
+          auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
+          if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
+            sensors_[stream_index]->start(profile,
+                                          [this, stream_index](std::shared_ptr<ob::Frame> frame) {
+                                            onNewIMUFrameCallback(frame, stream_index);
+                                          });
+            RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
+                                            << magic_enum::enum_name(gyro_range) << " range and "
+                                            << magic_enum::enum_name(gyro_rate) << " rate");
+            imu_started_[stream_index] = true;
+          }
+        }
+      }
+    }
+  }
+  for (const auto& stream_index : HID_STREAMS) {
+    if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
+                                       << magic_enum::enum_name(stream_index.first)
+                                       << ", please check the imu_rate and imu_range parameters");
+    }
+  }
 }
 
 void OBCameraNode::stopStreams() {
@@ -232,11 +284,22 @@ void OBCameraNode::stopStreams() {
   }
   try {
     pipeline_->stop();
+    stopIMU();
   } catch (const ob::Error& e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
   }
 }
 
+void OBCameraNode::stopIMU() {
+  for (const auto& stream_index : HID_STREAMS) {
+    if (imu_started_[stream_index]) {
+      CHECK(sensors_.count(stream_index));
+      sensors_[stream_index]->stop();
+      imu_started_[stream_index] = false;
+    }
+  }
+}
+
 void OBCameraNode::setupDefaultImageFormat() {
   format_[DEPTH] = OB_FORMAT_Y16;
   format_str_[DEPTH] = "Y16";
@@ -255,6 +318,7 @@ void OBCameraNode::setupDefaultImageFormat() {
 }
 
 void OBCameraNode::getParameters() {
+  setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "camera");
   for (auto stream_index : IMAGE_STREAMS) {
     std::string param_name = stream_name_[stream_index] + "_width";
     setAndGetNodeParameter(width_[stream_index], param_name, IMAGE_WIDTH);
@@ -264,11 +328,11 @@ void OBCameraNode::getParameters() {
     setAndGetNodeParameter(fps_[stream_index], param_name, IMAGE_FPS);
     param_name = "enable_" + stream_name_[stream_index];
     setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
-    param_name = stream_name_[stream_index] + "_frame_id";
-    std::string default_frame_id = "camera_" + stream_name_[stream_index] + "_frame";
+    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_" + stream_name_[stream_index] + "_optical_frame";
+        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);
     depth_aligned_frame_id_[stream_index] = stream_name_[COLOR] + "_optical_frame";
@@ -288,6 +352,26 @@ void OBCameraNode::getParameters() {
     param_name = stream_name_[stream_index] + "_camera_info_qos";
     setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
   }
+
+  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(enable_stream_[stream_index], param_name, false);
+    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);
+    depth_aligned_frame_id_[stream_index] = stream_name_[COLOR] + "_optical_frame";
+  }
+
   setAndGetNodeParameter(publish_tf_, "publish_tf", true);
   setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
   setAndGetNodeParameter(depth_registration_, "depth_registration", false);
@@ -319,6 +403,8 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<bool>(enable_ldp_, "enable_ldp", true);
   setAndGetNodeParameter<int>(soft_filter_max_diff_, "soft_filter_max_diff", -1);
   setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
+  setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
+  setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
 }
 
 void OBCameraNode::setupTopics() {
@@ -355,13 +441,13 @@ void OBCameraNode::setupPublishers() {
   using CameraInfo = sensor_msgs::msg::CameraInfo;
   auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
   if (enable_colored_point_cloud_) {
-    colored_point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
+    depth_registration_cloud_pub_ = node_->create_publisher<PointCloud2>(
         "depth/color/points",
         rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                     point_cloud_qos_profile));
   }
   if (enable_point_cloud_) {
-    point_cloud_publisher_ = node_->create_publisher<PointCloud2>(
+    depth_cloud_pub_ = node_->create_publisher<PointCloud2>(
         "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                                     point_cloud_qos_profile));
   }
@@ -382,6 +468,15 @@ void OBCameraNode::setupPublishers() {
         topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                            camera_info_qos_profile));
   }
+  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]);
+    imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
+        data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
+  }
   if (enable_publish_extrinsic_) {
     extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
         "extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local());
@@ -408,8 +503,8 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_
 }
 
 void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
-  if (!enable_point_cloud_ || !point_cloud_publisher_ ||
-      point_cloud_publisher_->get_subscription_count() == 0) {
+  if (!enable_point_cloud_ || !depth_cloud_pub_ ||
+      depth_cloud_pub_->get_subscription_count() == 0) {
     return;
   }
   if (!camera_param_ && depth_registration_) {
@@ -466,7 +561,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
   point_cloud_msg_.width = valid_count;
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
-  point_cloud_publisher_->publish(point_cloud_msg_);
+  depth_cloud_pub_->publish(point_cloud_msg_);
 
   if (save_point_cloud_) {
     save_point_cloud_ = false;
@@ -484,8 +579,8 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
 }
 
 void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set) {
-  if (!enable_colored_point_cloud_ || !colored_point_cloud_publisher_ ||
-      colored_point_cloud_publisher_->get_subscription_count() == 0) {
+  if (!enable_colored_point_cloud_ || !depth_registration_cloud_pub_ ||
+      depth_registration_cloud_pub_->get_subscription_count() == 0) {
     return;
   }
   auto depth_frame = frame_set->depthFrame();
@@ -556,7 +651,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
   point_cloud_msg_.width = valid_count;
   point_cloud_msg_.height = 1;
   modifier.resize(valid_count);
-  colored_point_cloud_publisher_->publish(point_cloud_msg_);
+  depth_registration_cloud_pub_->publish(point_cloud_msg_);
   if (save_colored_point_cloud_) {
     save_colored_point_cloud_ = false;
     auto now = std::time(nullptr);
@@ -695,6 +790,70 @@ void OBCameraNode::saveImageToFile(const stream_index_pair& stream_index, const
   }
 }
 
+void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
+                                         const stream_index_pair& stream_index) {
+  if (!imu_publishers_.count(stream_index)) {
+    RCLCPP_ERROR_STREAM(logger_,
+                        "stream " << stream_name_[stream_index] << " publisher not initialized");
+    return;
+  }
+  auto subscriber_count = imu_publishers_[stream_index]->get_subscription_count();
+  if (subscriber_count == 0) {
+    return;
+  }
+  auto imu_msg = sensor_msgs::msg::Imu();
+  setDefaultIMUMessage(imu_msg);
+  imu_msg.header.frame_id = optical_frame_id_[stream_index];
+  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
+  imu_msg.header.stamp = timestamp;
+  if (frame->type() == OB_FRAME_GYRO) {
+    auto gyro_frame = frame->as<ob::GyroFrame>();
+    auto data = gyro_frame->value();
+    imu_msg.angular_velocity.x = data.x;
+    imu_msg.angular_velocity.y = data.y;
+    imu_msg.angular_velocity.z = data.z;
+  } else if (frame->type() == OB_FRAME_ACCEL) {
+    auto accel_frame = frame->as<ob::AccelFrame>();
+    auto data = accel_frame->value();
+    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 OBCameraNode::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 = 0.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_};
+}
+
+sensor_msgs::msg::Imu OBCameraNode::createUnitIMUMessage(const IMUData& accel_data,
+                                                         const IMUData& gyro_data) {
+  sensor_msgs::msg::Imu imu_msg;
+  rclcpp::Time timestamp(gyro_data.timestamp_);
+  imu_msg.header.stamp = timestamp;
+  imu_msg.angular_velocity.x = gyro_data.data_.x();
+  imu_msg.angular_velocity.y = gyro_data.data_.y();
+  imu_msg.angular_velocity.z = gyro_data.data_.z();
+
+  imu_msg.linear_acceleration.x = accel_data.data_.x();
+  imu_msg.linear_acceleration.y = accel_data.data_.y();
+  imu_msg.linear_acceleration.z = accel_data.data_.z();
+  return imu_msg;
+}
+
 std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
   auto camera_params = device_->getCalibrationCameraParamList();
   for (size_t i = 0; i < camera_params->count(); i++) {
@@ -779,6 +938,8 @@ void OBCameraNode::calcAndPublishStaticTransform() {
   publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[DEPTH],
                   optical_frame_id_[DEPTH]);
   publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[DEPTH]);
+  publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[ACCEL]);
+  publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[GYRO]);
 }
 
 void OBCameraNode::publishStaticTransforms() {
@@ -812,6 +973,56 @@ void OBCameraNode::publishDynamicTransforms() {
   }
 }
 
+template <typename T>
+T lerp(const T& a, const T& b, const double t) {
+  return a * (1.0 - t) + b * t;
+}
+
+void OBCameraNode::FillImuDataLinearInterpolation(const IMUData& imu_data,
+                                                  std::deque<sensor_msgs::msg::Imu>& imu_msgs) {
+  imu_history_.push_back(imu_data);
+  stream_index_pair steam_index(imu_data.stream_);
+  imu_msgs.clear();
+  std::deque<IMUData> gyros_data;
+  IMUData accel0, accel1, current_imu;
+  while (!imu_history_.empty()) {
+    current_imu = imu_history_.front();
+    imu_history_.pop_front();
+    if (accel0.isSet() && current_imu.stream_ == ACCEL) {
+      accel0 = current_imu;
+    } else if (accel0.isSet() && current_imu.stream_ == ACCEL) {
+      accel1 = current_imu;
+      const double dt = accel1.timestamp_ - accel0.timestamp_;
+      while (!gyros_data.empty()) {
+        auto current_gyro = gyros_data.front();
+        gyros_data.pop_front();
+        const double alpha = (current_gyro.timestamp_ - accel0.timestamp_) / dt;
+        IMUData current_accel(ACCEL, lerp(accel0.data_, accel1.data_, alpha),
+                              current_gyro.timestamp_);
+        imu_msgs.push_back((createUnitIMUMessage(current_accel, current_gyro)));
+      }
+      accel0 = accel1;
+    } else if (accel0.isSet() && current_imu.timestamp_ >= accel0.timestamp_ &&
+               current_imu.stream_ == GYRO) {
+      gyros_data.push_back(current_imu);
+    }
+  }
+  imu_history_.push_back(current_imu);
+}
+
+void OBCameraNode::FillImuDataCopy(const IMUData& imu_data,
+                                   std::deque<sensor_msgs::msg::Imu>& imu_msgs) {
+  stream_index_pair steam_index(imu_data.stream_);
+  if (steam_index == ACCEL) {
+    accel_data_ = imu_data;
+    return;
+  }
+  if (accel_data_.isSet()) {
+    return;
+  }
+  imu_msgs.push_back(createUnitIMUMessage(accel_data_, imu_data));
+}
+
 bool OBCameraNode::setupFormatConvertType(OBFormat format) {
   switch (format) {
     case OB_FORMAT_RGB888:

orbbec_camera/src/synced_imu_publisher.cpp

@@ -0,0 +1,54 @@
+#include "orbbec_camera/utils.h"
+#include "orbbec_camera/synced_imu_publisher.h"
+#include <rclcpp/rclcpp.hpp>
+
+namespace orbbec_camera {
+SyncedImuPublisher::SyncedImuPublisher(
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher, size_t queue_size)
+    : imu_publisher_(imu_publisher), queue_size_(queue_size) {}
+
+SyncedImuPublisher::~SyncedImuPublisher() { publishPendingMessages(); }
+
+void SyncedImuPublisher::publish(const sensor_msgs::msg::Imu &imu_msg) {
+  std::unique_lock<std::mutex> lock(mutex_);
+  auto sub_num = imu_publisher_->get_subscription_count();
+  if (sub_num == 0 || !is_enabled_) {
+    return;
+  }
+  if (is_paused_) {
+    while (queue_.size() >= queue_size_) {
+      queue_.pop();
+    }
+    queue_.push(imu_msg);
+  } else {
+    imu_publisher_->publish(imu_msg);
+  }
+}
+
+void SyncedImuPublisher::pause() {
+  std::unique_lock<std::mutex> lock(mutex_);
+  is_paused_ = true;
+}
+
+void SyncedImuPublisher::resume() {
+  std::unique_lock<std::mutex> lock(mutex_);
+  is_paused_ = false;
+  publishPendingMessages();
+}
+
+void SyncedImuPublisher::setQueueSize(size_t queue_size) {
+  std::unique_lock<std::mutex> lock(mutex_);
+  queue_size_ = queue_size;
+}
+
+void SyncedImuPublisher::enable(bool enable) { is_enabled_ = enable; }
+
+void SyncedImuPublisher::publishPendingMessages() {
+  std::unique_lock<std::mutex> lock(mutex_);
+  while (!queue_.empty()) {
+    imu_publisher_->publish(queue_.front());
+    queue_.pop();
+  }
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/utils.cpp

@@ -256,12 +256,8 @@ bool isOpenNIDevice(int pid) {
       0x063a, 0x0650, 0x0651, 0x0654, 0x0655, 0x0656, 0x0657, 0x0658, 0x0659, 0x065a,
       0x065b, 0x065c, 0x065d, 0x0698, 0x0699, 0x069a};
 
-  for (const auto &pid_openni : OPENNI_DEVICE_PIDS) {
-    if (pid == pid_openni) {
-      return true;
-    }
-  }
-  return false;
+  return std::any_of(OPENNI_DEVICE_PIDS.begin(), OPENNI_DEVICE_PIDS.end(),
+                     [pid](int pid_openni) { return pid == pid_openni; });
 }
 
 OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
@@ -302,4 +298,86 @@ OBSyncMode OBSyncModeFromString(const std::string &mode) {
   }
 }
 
+OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
+  // covert to lower case
+  std::transform(sample_rate.begin(), sample_rate.end(), sample_rate.begin(), ::tolower);
+  if (sample_rate == "1.5625hz") {
+    return OB_SAMPLE_RATE_1_5625_HZ;
+  } else if (sample_rate == "3.125hz") {
+    return OB_SAMPLE_RATE_3_125_HZ;
+  } else if (sample_rate == "6.25hz") {
+    return OB_SAMPLE_RATE_6_25_HZ;
+  } else if (sample_rate == "12.5hz") {
+    return OB_SAMPLE_RATE_12_5_HZ;
+  } else if (sample_rate == "25hz") {
+    return OB_SAMPLE_RATE_25_HZ;
+  } else if (sample_rate == "50hz") {
+    return OB_SAMPLE_RATE_50_HZ;
+  } else if (sample_rate == "100hz") {
+    return OB_SAMPLE_RATE_100_HZ;
+  } else if (sample_rate == "200hz") {
+    return OB_SAMPLE_RATE_200_HZ;
+  } else if (sample_rate == "500hz") {
+    return OB_SAMPLE_RATE_500_HZ;
+  } else if (sample_rate == "1khz") {
+    return OB_SAMPLE_RATE_1_KHZ;
+  } else if (sample_rate == "2khz") {
+    return OB_SAMPLE_RATE_2_KHZ;
+  } else if (sample_rate == "4khz") {
+    return OB_SAMPLE_RATE_4_KHZ;
+  } else if (sample_rate == "8khz") {
+    return OB_SAMPLE_RATE_8_KHZ;
+  } else if (sample_rate == "16khz") {
+    return OB_SAMPLE_RATE_16_KHZ;
+  } else if (sample_rate == "32khz") {
+    return OB_SAMPLE_RATE_32_KHZ;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OB_SAMPLE_RATE: " << sample_rate);
+    return OB_SAMPLE_RATE_100_HZ;
+  }
+}
+
+OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string &full_scale_range) {
+  std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
+                 ::tolower);
+  if (full_scale_range == "16dps") {
+    return OB_GYRO_FS_16dps;
+  } else if (full_scale_range == "31dps") {
+    return OB_GYRO_FS_31dps;
+  } else if (full_scale_range == "62dps") {
+    return OB_GYRO_FS_62dps;
+  } else if (full_scale_range == "125dps") {
+    return OB_GYRO_FS_125dps;
+  } else if (full_scale_range == "250dps") {
+    return OB_GYRO_FS_250dps;
+  } else if (full_scale_range == "500dps") {
+    return OB_GYRO_FS_500dps;
+  } else if (full_scale_range == "1000dps") {
+    return OB_GYRO_FS_1000dps;
+  } else if (full_scale_range == "2000dps") {
+    return OB_GYRO_FS_2000dps;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
+                        "Unknown OB_GYRO_FULL_SCALE_RANGE: " << full_scale_range);
+    return OB_GYRO_FS_2000dps;
+  }
+}
+
+OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string &full_scale_range) {
+  std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
+                 ::tolower);
+  if (full_scale_range == "2g") {
+    return OB_ACCEL_FS_2g;
+  } else if (full_scale_range == "4g") {
+    return OB_ACCEL_FS_4g;
+  } else if (full_scale_range == "8g") {
+    return OB_ACCEL_FS_8g;
+  } else if (full_scale_range == "16g") {
+    return OB_ACCEL_FS_16g;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
+                        "Unknown OB_ACCEL_FULL_SCALE_RANGE: " << full_scale_range);
+    return OB_ACCEL_FS_16g;
+  }
+}
 }  // namespace orbbec_camera