devbox
喵喵爱编程
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

OrbbecSDK_ros关于imu数据的发布_orbbec pos sdk

作者：喵喵爱编程 | 2024-07-26 12:48:50

赞

踩

orbbec pos sdk

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

增加的代码都在setupDevices函数中

  1. void OBCameraNode::setupDevices() {
  2.   auto sensor_list = device_->getSensorList();
  3.   //std::cout<<"**********sensor list has "<<sensor_list->count()<<std::endl;
  4.  
  5.   for (size_t i = 0; i < sensor_list->count(); i++) {
  6.     auto sensor = sensor_list->getSensor(i);
  7.     //std::cout<<"**********sensor list  "<< i << " " <<sensor_list->type(i) <<std::endl;
  8.     
  9.     auto profiles = sensor->getStreamProfileList();
  10.     for (size_t j = 0; j < profiles->count(); j++) {
  11.       auto profile = profiles->getProfile(j);
  12.       stream_index_pair sip{profile->type(), 0};
  13.       if (sensors_.find(sip) != sensors_.end()) {
  14.         continue;
  15.       }
  16.       sensors_[sip] = std::make_shared<ROSOBSensor>(device_, sensor, stream_name_[sip]);
  17.     }
  18.   }
  19.   for (const auto& item : enable_stream_) {
  20.     auto stream_index = item.first;
  21.     auto enable = item.second;
  22.     if (enable && sensors_.find(stream_index) == sensors_.end()) {
  23.       ROS_INFO_STREAM(stream_name_[stream_index]
  24.                       << "sensor isn't supported by current device! -- Skipping...");
  25.       enable_stream_[stream_index] = false;
  26.     }
  27.   }
  28.   if (enable_d2c_viewer_) {
  29.     d2c_viewer_ = std::make_shared<D2CViewer>(nh_, nh_private_);
  30.   }
  31.   CHECK_NOTNULL(device_info_.get());
  32.   if (enable_pipeline_) {
  33.     pipeline_ = std::make_shared<ob::Pipeline>(device_);
  34.   }
  35.   try {
  36.     if (enable_hardware_d2d_ && device_info_->pid() == GEMINI2_PID) {
  37.       device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
  38.     }
  39.     if (!depth_work_mode_.empty()) {
  40.       device_->switchDepthWorkMode(depth_work_mode_.c_str());
  41.     }
  42.     if (sync_mode_ != OB_SYNC_MODE_CLOSE) {
  43.       OBDeviceSyncConfig sync_config;
  44.       sync_config.syncMode = sync_mode_;
  45.       sync_config.irTriggerSignalInDelay = ir_trigger_signal_in_delay_;
  46.       sync_config.rgbTriggerSignalInDelay = rgb_trigger_signal_in_delay_;
  47.       sync_config.deviceTriggerSignalOutDelay = device_trigger_signal_out_delay_;
  48.       device_->setSyncConfig(sync_config);
  49.       if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
  50.                                        OB_PERMISSION_READ_WRITE)) {
  51.         device_->setBoolProperty(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL, sync_signal_trigger_out_);
  52.       }
  53.     }
  54.     if (device_info_->pid() == GEMINI2_PID) {
  55.       auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
  56.       if (default_precision_level != depth_precision_) {
  57.         device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
  58.       }
  59.     }
  60.  
  61.     device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
  62.     device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
  63.     device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
  64.     auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
  65.     if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
  66.       device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
  67.     }
  68.     auto default_soft_filter_speckle_size =
  69.         device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
  70.     if (soft_filter_speckle_size_ != -1 &&
  71.         default_soft_filter_speckle_size != soft_filter_speckle_size_) {
  72.       device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
  73.     }
  74.  
  75.     //***************add imu*********************
  76.     enable_stream_[ACCEL] = true;
  77.     enable_stream_[GYRO] = true;
  78.  
  79.     for (const auto& stream_index : HID_STREAMS) {
  80.         if (stream_index == GYRO)
  81.         {
  82.             auto profile_list = sensors_[stream_index]->getStreamProfileList();
  83.             supported_profiles_[stream_index] = profile_list;
  84.             auto selected_profile = profile_list->getGyroStreamProfile(OB_GYRO_FS_125dps, OB_SAMPLE_RATE_200_HZ);
  85.             stream_profile_[stream_index] = selected_profile;
  86.         }
  87.         else if (stream_index == ACCEL)
  88.         {
  89.             auto profile_list = sensors_[stream_index]->getStreamProfileList();
  90.             supported_profiles_[stream_index] = profile_list;
  91.             auto selected_profile = profile_list->getAccelStreamProfile(OB_ACCEL_FS_4g, OB_SAMPLE_RATE_200_HZ);
  92.             stream_profile_[stream_index] = selected_profile;
  93.         }
  94.     }
  95.  
  96.     // for (const auto& stream_index : HID_STREAMS) {
  97.     //     if (enable_stream_[stream_index]) {
  98.     //         pipeline_config_->enableStream(stream_profile_[stream_index]);
  99.     //     }
  100.     // }
  101.  
  102.     imu_publisher_ = nh_.advertise<sensor_msgs::Imu>("imu_data", 10);
  103.     std::shared_ptr<ob::Sensor> gyroSensor  = nullptr;
  104.     std::shared_ptr<ob::Sensor> accelSensor = nullptr;
  105.     gyroSensor = device_->getSensorList()->getSensor(OB_SENSOR_GYRO);
  106.     if(gyroSensor){
  107.         auto profiles = gyroSensor->getStreamProfileList();
  108.         // auto profile = profiles->getProfile(0);
  109.         gyroSensor->start(stream_profile_[GYRO], [this](std::shared_ptr<ob::Frame> frame) {
  110.             std::unique_lock<std::mutex> lk(printerMutex);
  111.             //auto    timeStamp = frame->timeStamp();
  112.             gyr_timestamp = frame->timeStamp();
  113.             auto    gyroFrame = frame->as<ob::GyroFrame>();
  114.              if(gyroFrame != nullptr /*&& (timeStamp % 500) < 2*/) {  //( timeStamp % 500 ) < 2: Reduce printing frequency
  115.                 auto value = gyroFrame->value();
  116.                 // std::cout << "Gyro Frame: \n\r{\n\r"
  117.                 //               << "  tsp = " << timeStamp << "\n\r"
  118.                 //               << "  temperature = " << gyroFrame->temperature() << "\n\r"
  119.                 //               << "  gyro.x = " << value.x << " rad/s"
  120.                 //               << "\n\r"
  121.                 //               << "  gyro.y = " << value.y << " rad/s"
  122.                 //                << "\n\r"
  123.                 //               << "  gyro.z = " << value.z << " rad/s"
  124.                 //               << "\n\r"
  125.                 //sensor_msgs::Imu imu_msg = sensor_msgs::Imu();
  126.                 // std::cout<<"*******************************"<<std::endl;
  127.                 // std::cout<<"gyr_timestamp:"<< gyr_timestamp <<std::endl;
  128.                 imu_msg.header.stamp = ros::Time(gyr_timestamp);
  129.                 imu_msg.angular_velocity.x = value.x;
  130.                 imu_msg.angular_velocity.y = value.y;
  131.                 imu_msg.angular_velocity.z = value.z;
  132.                 // imu_publisher_.publish(imu_msg);
  133.                 }
  134.             });
  135.     }
  136.     accelSensor = device_->getSensorList()->getSensor(OB_SENSOR_ACCEL);
  137.     if(accelSensor) {
  138.         auto profiles = accelSensor->getStreamProfileList();
  139.         // auto profile = profiles->getProfile(0);
  140.         accelSensor->start(stream_profile_[ACCEL], [this](std::shared_ptr<ob::Frame> frame) {
  141.         std::unique_lock<std::mutex> lk(printerMutex);
  142.         //auto                         timeStamp  = frame->timeStamp();
  143.         acc_timestamp = frame->timeStamp();
  144.         auto accelFrame = frame->as<ob::AccelFrame>();
  145.         if(accelFrame != nullptr) {
  146.             auto value = accelFrame->value();
  147.             // std::cout << "Accel Frame: \n\r{\n\r"
  148.             //           << "  tsp = " << timeStamp << "\n\r"
  149.             //           << "  temperature = " << accelFrame->temperature() << "\n\r"
  150.             //           << "  accel.x = " << value.x << " m/s^2"
  151.             //           << "\n\r"
  152.             //           << "  accel.y = " << value.y << " m/s^2"
  153.             //           << "\n\r"
  154.             //           << "  accel.z = " << value.z << " m/s^2"
  155.             //           << "\n\r"
  156.             //           << "}\n\r" << std::endl;
  157.             // std::cout<<"acc_timestamp:"<< acc_timestamp <<std::endl;
  158.             // std::cout<<"*******************************"<<std::endl;
  159.             imu_msg.linear_acceleration.x = value.x;
  160.             imu_msg.linear_acceleration.y = value.y;
  161.             imu_msg.linear_acceleration.z = value.z;
  162.  
  163.             imu_publisher_.publish(imu_msg);
  164.             }
  165.         });
  166.     }
  167.     //*******************************************
  168.  
  169.   } catch (const ob::Error& e) {
  170.     ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage());
  171.   } catch (const std::exception& e) {
  172.     ROS_ERROR_STREAM("Failed to setup devices: " << e.what());
  173.   }
  174. }

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

  1. #include <iostream>
  2. #include <mutex>
  3. #include <libobsensor/ObSensor.hpp>
  4. #include "utils.hpp"
  5.  
  6. #define ESC 27
  7. std::mutex printerMutex;
  8.  
  9. int main(int argc, char **argv) try {
  10.     //打印SDK的版本号，SDK版本号分为主版本号，副版本号和修订版本号
  11.     // Print the SDK version number, the SDK version number is divided into major version number, minor version number and revision number
  12.     std::cout << "SDK version: " << ob::Version::getMajor() << "." << ob::Version::getMinor() << "." << ob::Version::getPatch() << std::endl;
  13.  
  14.     // Create a Context.
  15.     //首先需要创建一个Context，用于获取设备信息列表和创建设备
  16.     ob::Context ctx;
  17.  
  18.     // Query the list of connected devices
  19.     //查询已经接入设备的列表
  20.     auto devList = ctx.queryDeviceList();
  21.  
  22.     if(devList->deviceCount() == 0) {
  23.         std::cerr << "Device not found!" << std::endl;
  24.         return -1;
  25.     }
  26.  
  27.     // Create a device, 0 represents the index of the first device
  28.     auto                        dev         = devList->getDevice(0);
  29.     
  30.     std::shared_ptr<ob::Sensor> gyroSensor  = nullptr;
  31.     std::shared_ptr<ob::Sensor> accelSensor = nullptr;
  32.     try {
  33.         //获取陀螺仪传感器
  34.         gyroSensor = dev->getSensorList()->getSensor(OB_SENSOR_GYRO);
  35.         if(gyroSensor) {
  36.             //获取陀螺仪传感器的配置列表并选择第一个配置开流，在开流的回调里获取帧的数据
  37.             // Get configuration list
  38.             auto profiles = gyroSensor->getStreamProfileList();
  39.             // Select the first profile to open stream
  40.             auto profile = profiles->getProfile(0);
  41.             gyroSensor->start(profile, [](std::shared_ptr<ob::Frame> frame) {
  42.                 std::unique_lock<std::mutex> lk(printerMutex);
  43.                 auto    timeStamp = frame->timeStamp();
  44.                 auto    gyroFrame = frame->as<ob::GyroFrame>();
  45.                 if(gyroFrame != nullptr ) {  //( timeStamp % 500 ) < 2: Reduce printing frequency
  46.                     auto value = gyroFrame->value();
  47.                     std::cout << "Gyro Frame: \n\r{\n\r"
  48.                               << "  timestamp = " << timeStamp << "\n\r"
  49.                               << "  temperature = " << gyroFrame->temperature() << "\n\r"
  50.                               << "  gyro.x = " << value.x << " rad/s"
  51.                               << "\n\r"
  52.                               << "  gyro.y = " << value.y << " rad/s"
  53.                                << "\n\r"
  54.                               << "  gyro.z = " << value.z << " rad/s"
  55.                               << "\n\r"
  56.                               << "}\n\r" << std::endl;
  57.                 }
  58.             });
  59.         }
  60.         else {
  61.             std::cout << "get gyro Sensor failed ! " << std::endl;
  62.         }
  63.     }
  64.     catch(ob::Error &e) {
  65.         std::cerr << "current device is not support imu!" << std::endl;
  66.         exit(EXIT_FAILURE);
  67.     }
  68.         
  69.     // Get Acceleration Sensor
  70.     accelSensor = dev->getSensorList()->getSensor(OB_SENSOR_ACCEL);
  71.     if(accelSensor) {
  72.         // Get configuration list
  73.         auto profiles = accelSensor->getStreamProfileList();
  74.         // Select the first profile to open stream
  75.         auto profile = profiles->getProfile(0);
  76.         accelSensor->start(profile, [](std::shared_ptr<ob::Frame> frame) {
  77.             std::unique_lock<std::mutex> lk(printerMutex);
  78.             auto                         timeStamp  = frame->timeStamp();
  79.             auto                         accelFrame = frame->as<ob::AccelFrame>();
  80.             if(accelFrame != nullptr ) {
  81.                 auto value = accelFrame->value();
  82.                 std::cout << "Accel Frame: \n\r{\n\r"
  83.                           << "  timestamp = " << timeStamp << "\n\r"
  84.                           << "  temperature = " << accelFrame->temperature() << "\n\r"
  85.                           << "  accel.x = " << value.x << " m/s^2"
  86.                           << "\n\r"
  87.                           << "  accel.y = " << value.y << " m/s^2"
  88.                           << "\n\r"
  89.                           << "  accel.z = " << value.z << " m/s^2"
  90.                           << "\n\r"
  91.                           << "}\n\r" << std::endl;
  92.             }
  93.         });
  94.     }
  95.     else {
  96.         std::cout << "get Accel Sensor failed ! " << std::endl;
  97.     }
  98.  
  99.     std::cout << "Press ESC to exit! " << std::endl;
  100.  
  101.     while(true) {
  102.         // Get the value of the pressed key, if it is the ESC key, exit the program
  103.         int key = getch();
  104.         if(key == ESC)
  105.             break;
  106.     }
  107.  
  108.     // turn off the flow
  109.     if(gyroSensor) {
  110.         gyroSensor->stop();
  111.     }
  112.     if(accelSensor) {
  113.         accelSensor->stop();
  114.     }
  115.  
  116.     return 0;
  117. }
  118. catch(ob::Error &e) {
  119.     std::cerr << "function:" << e.getName() << "\nargs:" << e.getArgs() << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType() << std::endl;
  120.     exit(EXIT_FAILURE);
  121. }

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/喵喵爱编程/article/detail/885366
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号