OrbbecSDK_ros关于imu数据的发布

orbbec官方提供ros版本的sdk还不支持imu数据获取，貌似下个版本更新后就可以得到imu数据的话题。参考c++版本的sdk对ros的sdk进行修改，最后将imu数据以topic的形式发布。

增加的代码都在setupDevices函数中

void OBCameraNode::setupDevices() {
  auto sensor_list = device_->getSensorList();
  //std::cout<<"**********sensor list has "<<sensor_list->count()<<std::endl;

  for (size_t i = 0; i < sensor_list->count(); i++) {
    auto sensor = sensor_list->getSensor(i);
    //std::cout<<"**********sensor list  "<< i << " " <<sensor_list->type(i) <<std::endl;
    
    auto profiles = sensor->getStreamProfileList();
    for (size_t j = 0; j < profiles->count(); j++) {
      auto profile = profiles->getProfile(j);
      stream_index_pair sip{profile->type(), 0};
      if (sensors_.find(sip) != sensors_.end()) {
        continue;
      }
      sensors_[sip] = std::make_shared<ROSOBSensor>(device_, sensor, stream_name_[sip]);
    }
  }
  for (const auto& item : enable_stream_) {
    auto stream_index = item.first;
    auto enable = item.second;
    if (enable && sensors_.find(stream_index) == sensors_.end()) {
      ROS_INFO_STREAM(stream_name_[stream_index]
                      << "sensor isn't supported by current device! -- Skipping...");
      enable_stream_[stream_index] = false;
    }
  }
  if (enable_d2c_viewer_) {
    d2c_viewer_ = std::make_shared<D2CViewer>(nh_, nh_private_);
  }
  CHECK_NOTNULL(device_info_.get());
  if (enable_pipeline_) {
    pipeline_ = std::make_shared<ob::Pipeline>(device_);
  }
  try {
    if (enable_hardware_d2d_ && device_info_->pid() == GEMINI2_PID) {
      device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
    }
    if (!depth_work_mode_.empty()) {
      device_->switchDepthWorkMode(depth_work_mode_.c_str());
    }
    if (sync_mode_ != OB_SYNC_MODE_CLOSE) {
      OBDeviceSyncConfig sync_config;
      sync_config.syncMode = sync_mode_;
      sync_config.irTriggerSignalInDelay = ir_trigger_signal_in_delay_;
      sync_config.rgbTriggerSignalInDelay = rgb_trigger_signal_in_delay_;
      sync_config.deviceTriggerSignalOutDelay = device_trigger_signal_out_delay_;
      device_->setSyncConfig(sync_config);
      if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
                                       OB_PERMISSION_READ_WRITE)) {
        device_->setBoolProperty(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL, sync_signal_trigger_out_);
      }
    }
    if (device_info_->pid() == GEMINI2_PID) {
      auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
      if (default_precision_level != depth_precision_) {
        device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
      }
    }

    device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
    device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
    device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
    auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
    if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
      device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
    }
    auto default_soft_filter_speckle_size =
        device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
    if (soft_filter_speckle_size_ != -1 &&
        default_soft_filter_speckle_size != soft_filter_speckle_size_) {
      device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
    }

    //***************add imu*********************
    enable_stream_[ACCEL] = true;
    enable_stream_[GYRO] = true;

    for (const auto& stream_index : HID_STREAMS) {
        if (stream_index == GYRO)
        {
            auto profile_list = sensors_[stream_index]->getStreamProfileList();
            supported_profiles_[stream_index] = profile_list;
            auto selected_profile = profile_list->getGyroStreamProfile(OB_GYRO_FS_125dps, OB_SAMPLE_RATE_200_HZ);
            stream_profile_[stream_index] = selected_profile;
        }
        else if (stream_index == ACCEL)
        {
            auto profile_list = sensors_[stream_index]->getStreamProfileList();
            supported_profiles_[stream_index] = profile_list;
            auto selected_profile = profile_list->getAccelStreamProfile(OB_ACCEL_FS_4g, OB_SAMPLE_RATE_200_HZ);
            stream_profile_[stream_index] = selected_profile;
        }
    }

    // for (const auto& stream_index : HID_STREAMS) {
    //     if (enable_stream_[stream_index]) {
    //         pipeline_config_->enableStream(stream_profile_[stream_index]);
    //     }
    // }

    imu_publisher_ = nh_.advertise<sensor_msgs::Imu>("imu_data", 10);
    std::shared_ptr<ob::Sensor> gyroSensor  = nullptr;
    std::shared_ptr<ob::Sensor> accelSensor = nullptr;
    gyroSensor = device_->getSensorList()->getSensor(OB_SENSOR_GYRO);
    if(gyroSensor){
        auto profiles = gyroSensor->getStreamProfileList();
        // auto profile = profiles->getProfile(0);
        gyroSensor->start(stream_profile_[GYRO], [this](std::shared_ptr<ob::Frame> frame) {
            std::unique_lock<std::mutex> lk(printerMutex);
            //auto    timeStamp = frame->timeStamp();
            gyr_timestamp = frame->timeStamp();
            auto    gyroFrame = frame->as<ob::GyroFrame>();
             if(gyroFrame != nullptr /*&& (timeStamp % 500) < 2*/) {  //( timeStamp % 500 ) < 2: Reduce printing frequency
                auto value = gyroFrame->value();
                // std::cout << "Gyro Frame: \n\r{\n\r"
                //               << "  tsp = " << timeStamp << "\n\r"
                //               << "  temperature = " << gyroFrame->temperature() << "\n\r"
                //               << "  gyro.x = " << value.x << " rad/s"
                //               << "\n\r"
                //               << "  gyro.y = " << value.y << " rad/s"
                //                << "\n\r"
                //               << "  gyro.z = " << value.z << " rad/s"
                //               << "\n\r"
                //sensor_msgs::Imu imu_msg = sensor_msgs::Imu();
                // std::cout<<"*******************************"<<std::endl;
                // std::cout<<"gyr_timestamp:"<< gyr_timestamp <<std::endl;
                imu_msg.header.stamp = ros::Time(gyr_timestamp);
                imu_msg.angular_velocity.x = value.x;
                imu_msg.angular_velocity.y = value.y;
                imu_msg.angular_velocity.z = value.z;
                // imu_publisher_.publish(imu_msg);
                }
            });
    }
    accelSensor = device_->getSensorList()->getSensor(OB_SENSOR_ACCEL);
    if(accelSensor) {
        auto profiles = accelSensor->getStreamProfileList();
        // auto profile = profiles->getProfile(0);
        accelSensor->start(stream_profile_[ACCEL], [this](std::shared_ptr<ob::Frame> frame) {
        std::unique_lock<std::mutex> lk(printerMutex);
        //auto                         timeStamp  = frame->timeStamp();
        acc_timestamp = frame->timeStamp();
        auto accelFrame = frame->as<ob::AccelFrame>();
        if(accelFrame != nullptr) {
            auto value = accelFrame->value();
            // std::cout << "Accel Frame: \n\r{\n\r"
            //           << "  tsp = " << timeStamp << "\n\r"
            //           << "  temperature = " << accelFrame->temperature() << "\n\r"
            //           << "  accel.x = " << value.x << " m/s^2"
            //           << "\n\r"
            //           << "  accel.y = " << value.y << " m/s^2"
            //           << "\n\r"
            //           << "  accel.z = " << value.z << " m/s^2"
            //           << "\n\r"
            //           << "}\n\r" << std::endl;
            // std::cout<<"acc_timestamp:"<< acc_timestamp <<std::endl;
            // std::cout<<"*******************************"<<std::endl;
            imu_msg.linear_acceleration.x = value.x;
            imu_msg.linear_acceleration.y = value.y;
            imu_msg.linear_acceleration.z = value.z;

            imu_publisher_.publish(imu_msg);
            }
        });
    }
    //*******************************************

  } catch (const ob::Error& e) {
    ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage());
  } catch (const std::exception& e) {
    ROS_ERROR_STREAM("Failed to setup devices: " << e.what());
  }
}

参考的c++_sdk关于imu数据获取代码

#include <iostream>
#include <mutex>
#include <libobsensor/ObSensor.hpp>
#include "utils.hpp"

#define ESC 27
std::mutex printerMutex;

int main(int argc, char **argv) try {
    //打印SDK的版本号，SDK版本号分为主版本号，副版本号和修订版本号
    // Print the SDK version number, the SDK version number is divided into major version number, minor version number and revision number
    std::cout << "SDK version: " << ob::Version::getMajor() << "." << ob::Version::getMinor() << "." << ob::Version::getPatch() << std::endl;

    // Create a Context.
    //首先需要创建一个Context，用于获取设备信息列表和创建设备
    ob::Context ctx;

    // Query the list of connected devices
    //查询已经接入设备的列表
    auto devList = ctx.queryDeviceList();

    if(devList->deviceCount() == 0) {
        std::cerr << "Device not found!" << std::endl;
        return -1;
    }

    // Create a device, 0 represents the index of the first device
    auto                        dev         = devList->getDevice(0);
    
    std::shared_ptr<ob::Sensor> gyroSensor  = nullptr;
    std::shared_ptr<ob::Sensor> accelSensor = nullptr;
    try {
        //获取陀螺仪传感器
        gyroSensor = dev->getSensorList()->getSensor(OB_SENSOR_GYRO);
        if(gyroSensor) {
            //获取陀螺仪传感器的配置列表并选择第一个配置开流，在开流的回调里获取帧的数据
            // Get configuration list
            auto profiles = gyroSensor->getStreamProfileList();
            // Select the first profile to open stream
            auto profile = profiles->getProfile(0);
            gyroSensor->start(profile, [](std::shared_ptr<ob::Frame> frame) {
                std::unique_lock<std::mutex> lk(printerMutex);
                auto    timeStamp = frame->timeStamp();
                auto    gyroFrame = frame->as<ob::GyroFrame>();
                if(gyroFrame != nullptr ) {  //( timeStamp % 500 ) < 2: Reduce printing frequency
                    auto value = gyroFrame->value();
                    std::cout << "Gyro Frame: \n\r{\n\r"
                              << "  timestamp = " << timeStamp << "\n\r"
                              << "  temperature = " << gyroFrame->temperature() << "\n\r"
                              << "  gyro.x = " << value.x << " rad/s"
                              << "\n\r"
                              << "  gyro.y = " << value.y << " rad/s"
                               << "\n\r"
                              << "  gyro.z = " << value.z << " rad/s"
                              << "\n\r"
                              << "}\n\r" << std::endl;
                }
            });
        }
        else {
            std::cout << "get gyro Sensor failed ! " << std::endl;
        }
    }
    catch(ob::Error &e) {
        std::cerr << "current device is not support imu!" << std::endl;
        exit(EXIT_FAILURE);
    }
        
    // Get Acceleration Sensor
    accelSensor = dev->getSensorList()->getSensor(OB_SENSOR_ACCEL);
    if(accelSensor) {
        // Get configuration list
        auto profiles = accelSensor->getStreamProfileList();
        // Select the first profile to open stream
        auto profile = profiles->getProfile(0);
        accelSensor->start(profile, [](std::shared_ptr<ob::Frame> frame) {
            std::unique_lock<std::mutex> lk(printerMutex);
            auto                         timeStamp  = frame->timeStamp();
            auto                         accelFrame = frame->as<ob::AccelFrame>();
            if(accelFrame != nullptr ) {
                auto value = accelFrame->value();
                std::cout << "Accel Frame: \n\r{\n\r"
                          << "  timestamp = " << timeStamp << "\n\r"
                          << "  temperature = " << accelFrame->temperature() << "\n\r"
                          << "  accel.x = " << value.x << " m/s^2"
                          << "\n\r"
                          << "  accel.y = " << value.y << " m/s^2"
                          << "\n\r"
                          << "  accel.z = " << value.z << " m/s^2"
                          << "\n\r"
                          << "}\n\r" << std::endl;
            }
        });
    }
    else {
        std::cout << "get Accel Sensor failed ! " << std::endl;
    }

    std::cout << "Press ESC to exit! " << std::endl;

    while(true) {
        // Get the value of the pressed key, if it is the ESC key, exit the program
        int key = getch();
        if(key == ESC)
            break;
    }

    // turn off the flow
    if(gyroSensor) {
        gyroSensor->stop();
    }
    if(accelSensor) {
        accelSensor->stop();
    }

    return 0;
}
catch(ob::Error &e) {
    std::cerr << "function:" << e.getName() << "\nargs:" << e.getArgs() << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType() << std::endl;
    exit(EXIT_FAILURE);
}