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 "<count()<count(); i++) {
auto sensor = sensor_list->getSensor(i);
//std::cout<<"**********sensor list "<< i << " " <type(i) <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(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(nh_, nh_private_);
}
CHECK_NOTNULL(device_info_.get());
if (enable_pipeline_) {
pipeline_ = std::make_shared(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("imu_data", 10);
std::shared_ptr gyroSensor = nullptr;
std::shared_ptr 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 frame) {
std::unique_lock lk(printerMutex);
//auto timeStamp = frame->timeStamp();
gyr_timestamp = frame->timeStamp();
auto gyroFrame = frame->as();
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<<"*******************************"<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 frame) {
std::unique_lock lk(printerMutex);
//auto timeStamp = frame->timeStamp();
acc_timestamp = frame->timeStamp();
auto accelFrame = frame->as();
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 < 参考的c++_sdk关于imu数据获取代码
#include
#include
#include
#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 gyroSensor = nullptr;
std::shared_ptr 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 frame) {
std::unique_lock lk(printerMutex);
auto timeStamp = frame->timeStamp();
auto gyroFrame = frame->as();
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 frame) {
std::unique_lock lk(printerMutex);
auto timeStamp = frame->timeStamp();
auto accelFrame = frame->as();
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);
}