boost解析io转换成为imu_msgs

本文介绍如何使用SYDDynamics的9轴AHRS通过串口连接Ubuntu系统,并发布sensor_msgs/Imu类型消息。文中详细展示了C++代码实现过程,包括串口配置、请求四元数数据、发布ROS话题等步骤。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

下面使用SYD Dynamics的9轴AHRS( Attitude and heading reference system),来发布sensor_msgs/Imu类型的消息。

   将传感器用USB转串口接到Ubuntu系统上,可以用如下命令查看串口信息:

ls -l /dev/tty*

   查询出串口名为“/dev/ttyUSB0”。根据官方给的传感器程序源文件和boost::asio库来实现串口发送request指令,并读取传感器返回的四元数信息。之后将其发送到/IMU_data的话题上:

复制代码
// Step 1:  Include Library Headers:
#include "EasyObjectDictionary.h"
#include "EasyProfile.h"

#include <ros/ros.h> 
#include <sensor_msgs/Imu.h>

#include <boost/asio.hpp>         // 包含boost库函数
#include <boost/bind.hpp>
using namespace boost::asio;  


int main(int argc, char** argv)
{
    // Step 2: Initialization:
    EasyProfile_C_Interface_Init();

    ros::init(argc, argv, "imu");     
    ros::NodeHandle n;  

    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);  

    io_service io;
    serial_port sp(io, "/dev/ttyUSB0");         // 定义传输的串口
    sp.set_option(serial_port::baud_rate(115200));                                  // 波特率
    sp.set_option(  serial_port::flow_control( serial_port::flow_control::none ) ); // 流量控制
    sp.set_option(  serial_port::parity( serial_port::parity::none ) );             // 奇偶校验
    sp.set_option( serial_port::stop_bits(  serial_port::stop_bits::one ) );        // 停止位
    sp.set_option(  serial_port::character_size( 8 ) );                             // 数据位

    ros::Rate loop_rate(50);  
    while(ros::ok())
    {
        // Step 3 and Step 4 are optional, only if you want to use the request-response communication pattern
        // Step 3: Request quaternion Data from TransdcuerM 
        uint16 toId = 309; // Node ID
        char*  txData;
        int    txSize;
        if(EP_SUCC_ == EasyProfile_C_Interface_TX_Request(toId, EP_CMD_Q_S1_E_, &txData, &txSize))
        {
            write(sp, buffer(txData, txSize));    // Step 4:  Send the request via Serial Port.
        } 

        char rxData[100];  
        boost::system::error_code err;  

        sp.read_some(buffer(rxData, 50),err);  // Read from serial port buffer
        if (err)  
        {  
            ROS_INFO("Serial port read_some Error!");  
            return -1;  
        }  

        Ep_Header header;                       // Then let the EasyProfile do the rest such as data assembling and checksum verification.
        if( EP_SUCC_ == EasyProfile_C_Interface_RX((char*)rxData, 50, &header))
        {
            // Quanternion received
            unsigned int timeStamp = ep_Q_s1_e.timeStamp;
            float q1 = ep_Q_s1_e.q[0];              // Note 1, ep_Q_s1_e is defined in the EasyProfile library as a global variable
            float q2 = ep_Q_s1_e.q[1];              // Note 2, for the units and meaning of each value, refer to EasyObjectDictionary.h
            float q3 = ep_Q_s1_e.q[2];
            float q4 = ep_Q_s1_e.q[3];
            ROS_INFO("Q: %f %f %f %f\n", q1, q2, q3, q4);

            sensor_msgs::Imu imu_data;
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            imu_data.orientation.x = q3;
            imu_data.orientation.y = -q2;
            imu_data.orientation.z = -q1;
            imu_data.orientation.w = q4;

            IMU_pub.publish(imu_data);
        }

        io.run();  
        ros::spinOnce();  
        loop_rate.sleep();  
    }

    return 0;
}
复制代码

   在CMakeLists中添加:

add_compile_options(-std=c99)

aux_source_directory(./src  DIR_SRCS)
add_executable(imu   ${DIR_SRCS} )
target_link_libraries(imu  ${catkin_LIBRARIES})

  使用catkin_make编译后,source ./devel/setup.bash,然后运行rosrun imu imu。这时可能会出现无法打开串口的错误,给串口添加权限后再次运行: 

sudo chmod 666 /dev/ttyUSB0

   无问题后可以输入下面的指令查看话题,转动IMU可以看到orientation的四个分量一直在变化:

rostopic  echo  /IMU_data

 

   为了更形象的显示IMU姿态,可以下载rviz_imu_plugin插件并安装。The rviz_imu_plugin package is used to display sensor_msgs/Imu messages in rviz. Once you download and compile the package, it should be visible as a plugin. It displays the orientation of the IMU using a box as well as and coordinate axes. The acceleration can be visualized using a vector.

  Make sure you have git installed:

sudo apt-get install git-core

  Download the stack from our repository into your catkin workspace

git clone -b indigo https://github.com/ccny-ros-pkg/imu_tools.git

  Compile the stack:

cd ~/catkin_ws
catkin_make

   装好后打开rviz,可以看到rviz_imu_plugin与rviz中默认自带的rviz_plugin_tutorials并不一样:

   在rviz_imu_plugin下添加imu,修改Fixed Frame为base_link,IMU下面的Topic选为/IMU_data,转动IMU rviz中的虚拟立方体和坐标轴会跟着转动(可以更改box三个方向尺寸的比例):

 

参考:

IMU tools for ROS

CMake 入门实战

ROS Qt Creator Plug-in wiki

ROS下IMU串口通讯接口(通用版)

如何使用Qt插件在Qt中进行ROS开发

如何用Qt对ROS项目进行调试及创建GUI界面

Serial Cross-platform, Serial Port library written in C++

lixing@lixing:~/fast_livo2$ catkin_make Base path: /home/lixing/fast_livo2 Source space: /home/lixing/fast_livo2/src Build space: /home/lixing/fast_livo2/build Devel space: /home/lixing/fast_livo2/devel Install space: /home/lixing/fast_livo2/install #### #### Running command: "make cmake_check_build_system" in "/home/lixing/fast_livo2/build" #### -- Using CATKIN_DEVEL_PREFIX: /home/lixing/fast_livo2/devel -- Using CMAKE_PREFIX_PATH: /opt/ros/noetic -- This workspace overlays: /opt/ros/noetic -- Found PythonInterp: /usr/bin/python3 (found suitable version "3.8.10", minimum required is "3") -- Using PYTHON_EXECUTABLE: /usr/bin/python3 -- Using Debian Python package layout -- Using empy: /usr/lib/python3/dist-packages/em.py -- Using CATKIN_ENABLE_TESTING: ON -- Call enable_testing() -- Using CATKIN_TEST_RESULTS_DIR: /home/lixing/fast_livo2/build/test_results -- Forcing gtest/gmock from source, though one was otherwise available. -- Found gtest sources under '/usr/src/googletest': gtests will be built -- Found gmock sources under '/usr/src/googletest': gmock will be built -- Found PythonInterp: /usr/bin/python3 (found version "3.8.10") -- Using Python nosetests: /usr/bin/nosetests3 -- catkin 0.8.12 -- BUILD_SHARED_LIBS is on -- BUILD_SHARED_LIBS is on -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- ~~ traversing 5 packages in topological order: -- ~~ - livox_ros_driver2 -- ~~ - vikit_common -- ~~ - vikit_py -- ~~ - vikit_ros -- ~~ - fast_livo -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- +++ processing catkin package: 'livox_ros_driver2' -- ==> add_subdirectory(livox_ros_driver2) -- livox_ros_driver2 version: 1.0.0 -- Found Boost: /usr/lib/x86_64-linux-gnu/cmake/Boost-1.71.0/BoostConfig.cmake (found suitable version "1.71.0", minimum required is "1.54") found components: system thread chrono -- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy -- Eigen found (include: /usr/include/eigen3, version: 3.3.7) -- Found Boost: /usr/include (found suitable version "1.71.0", minimum required is "1.55.0") found components: system filesystem date_time iostreams regex -- The imported target "vtkParseOGLExt" references the file "/usr/bin/vtkParseOGLExt-7.1" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "vtkRenderingPythonTkWidgets" references the file "/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "vtk" references the file "/usr/bin/vtk" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "pvtk" references the file "/usr/bin/pvtk" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- OpenNI2 found (include: /usr/include/openni2, lib: /usr/lib/libOpenNI2.so) ** WARNING ** io features related to pcap will be disabled ** WARNING ** io features related to png will be disabled ** WARNING ** io features related to libusb-1.0 will be disabled -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- OpenNI2 found (include: /usr/include/openni2, lib: /usr/lib/libOpenNI2.so) -- QHULL found (include: /usr/include, lib: optimized;/usr/lib/x86_64-linux-gnu/libqhull.so;debug;/usr/lib/x86_64-linux-gnu/libqhull.so) -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- looking for PCL_COMMON -- looking for PCL_KDTREE -- looking for PCL_OCTREE -- looking for PCL_SEARCH -- looking for PCL_SAMPLE_CONSENSUS -- looking for PCL_FILTERS -- looking for PCL_2D -- looking for PCL_GEOMETRY -- looking for PCL_IO -- looking for PCL_FEATURES -- looking for PCL_ML -- looking for PCL_SEGMENTATION -- looking for PCL_VISUALIZATION -- looking for PCL_SURFACE -- looking for PCL_REGISTRATION -- looking for PCL_KEYPOINTS -- looking for PCL_TRACKING -- looking for PCL_RECOGNITION -- looking for PCL_STEREO -- looking for PCL_APPS -- looking for PCL_IN_HAND_SCANNER -- looking for PCL_POINT_CLOUD_EDITOR -- looking for PCL_OUTOFCORE -- looking for PCL_PEOPLE -- livox_ros_driver2: 2 messages, 0 services /usr/include/apr-1.0 apr-1 -- +++ processing catkin package: 'vikit_common' -- ==> add_subdirectory(rpg_vikit/vikit_common) Current CPU archtecture: x86_64 -- Found Eigen: /usr/include/eigen3 -- Eigen found (include: /usr/include/eigen3) -- +++ processing catkin package: 'vikit_py' -- ==> add_subdirectory(rpg_vikit/vikit_py) -- +++ processing catkin package: 'vikit_ros' -- ==> add_subdirectory(rpg_vikit/vikit_ros) -- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy -- +++ processing catkin package: 'fast_livo' -- ==> add_subdirectory(fast-livo2-native) -- Build Type: Release -- Current CPU architecture: x86_64 -- Using general x86 optimizations: -O3 -march=native -mtune=native -funroll-loops -- Processor count: 8 -- Multithreading enabled. Cores: 4 -- OpenMP found -- mimalloc not found, proceeding without it -- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy -- Found Eigen: /usr/include/eigen3 (Required is at least version "3.1") -- Eigen found (include: /usr/include/eigen3, version: 3.3.7) -- The imported target "vtkParseOGLExt" references the file "/usr/bin/vtkParseOGLExt-7.1" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "vtkRenderingPythonTkWidgets" references the file "/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "vtk" references the file "/usr/bin/vtk" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- The imported target "pvtk" references the file "/usr/bin/pvtk" but this file does not exist. Possible reasons include: * The file was deleted, renamed, or moved to another location. * An install or uninstall procedure did not complete successfully. * The installation package was faulty and contained "/usr/lib/cmake/vtk-7.1/VTKTargets.cmake" but not all the files it references. -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- OpenNI2 found (include: /usr/include/openni2, lib: /usr/lib/libOpenNI2.so) ** WARNING ** io features related to pcap will be disabled ** WARNING ** io features related to png will be disabled ** WARNING ** io features related to libusb-1.0 will be disabled -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- OpenNI2 found (include: /usr/include/openni2, lib: /usr/lib/libOpenNI2.so) -- QHULL found (include: /usr/include, lib: optimized;/usr/lib/x86_64-linux-gnu/libqhull.so;debug;/usr/lib/x86_64-linux-gnu/libqhull.so) -- OpenNI found (include: /usr/include/ni, lib: /usr/lib/libOpenNI.so) -- looking for PCL_COMMON -- looking for PCL_KDTREE -- looking for PCL_OCTREE -- looking for PCL_SEARCH -- looking for PCL_SAMPLE_CONSENSUS -- looking for PCL_FILTERS -- looking for PCL_2D -- looking for PCL_GEOMETRY -- looking for PCL_IO -- looking for PCL_FEATURES -- looking for PCL_ML -- looking for PCL_SEGMENTATION -- looking for PCL_VISUALIZATION -- looking for PCL_SURFACE -- looking for PCL_REGISTRATION -- looking for PCL_KEYPOINTS -- looking for PCL_TRACKING -- looking for PCL_RECOGNITION -- looking for PCL_STEREO -- looking for PCL_APPS -- looking for PCL_IN_HAND_SCANNER -- looking for PCL_POINT_CLOUD_EDITOR -- looking for PCL_OUTOFCORE -- looking for PCL_PEOPLE -- Found Boost: /usr/include (found version "1.71.0") found components: thread chrono date_time atomic -- Configuring done -- Generating done -- Build files have been written to: /home/lixing/fast_livo2/build #### #### Running command: "make -j8 -l8" in "/home/lixing/fast_livo2/build" #### [ 0%] Built target std_msgs_generate_messages_cpp [ 0%] Built target std_msgs_generate_messages_nodejs [ 0%] Built target std_msgs_generate_messages_eus [ 0%] Built target std_msgs_generate_messages_lisp [ 0%] Built target std_msgs_generate_messages_py [ 20%] Built target vikit_common [ 20%] Built target _livox_ros_driver2_generate_messages_check_deps_CustomMsg [ 22%] Building CXX object fast-livo2-native/CMakeFiles/imu_proc.dir/src/IMU_Processing.cpp.o [ 25%] Built target pre [ 25%] Built target _livox_ros_driver2_generate_messages_check_deps_CustomPoint [ 26%] Building CXX object fast-livo2-native/CMakeFiles/laser_mapping.dir/src/LIVMapper.cpp.o [ 30%] Built target lio [ 36%] Built target vio [ 39%] Built target test_vk_common_camera [ 42%] Built target test_vk_common_patch_score [ 46%] Built target test_vk_common_triangulation [ 49%] Built target vikit_ros [ 52%] Built target livox_ros_driver2_generate_messages_cpp [ 55%] Built target livox_ros_driver2_generate_messages_nodejs [ 58%] Built target livox_ros_driver2_generate_messages_lisp [ 63%] Built target livox_ros_driver2_generate_messages_eus [ 68%] Built target livox_ros_driver2_generate_messages_py [ 93%] Built target livox_ros_driver2_node [ 93%] Built target livox_ros_driver2_generate_messages In file included from /home/lixing/fast_livo2/src/fast-livo2-native/src/IMU_Processing.cpp:13: /home/lixing/fast_livo2/src/fast-livo2-native/include/IMU_Processing.h:52:12: error: field ‘fout_imu’ has incomplete type ‘std::ofstream’ {aka ‘std::basic_ofstream<char>’} 52 | ofstream fout_imu; | ^~~~~~~~ In file included from /usr/include/c++/9/ios:38, from /usr/include/c++/9/istream:38, from /usr/include/c++/9/sstream:38, from /usr/include/c++/9/complex:45, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/Dense:1, from /usr/include/eigen3/Eigen/Eigen:1, from /home/lixing/fast_livo2/src/fast-livo2-native/include/IMU_Processing.h:16, from /home/lixing/fast_livo2/src/fast-livo2-native/src/IMU_Processing.cpp:13: /usr/include/c++/9/iosfwd:119:11: note: declaration of ‘std::ofstream’ {aka ‘class std::basic_ofstream<char>’} 119 | class basic_ofstream; | ^~~~~~~~~~~~~~ In file included from /home/lixing/fast_livo2/src/fast-livo2-native/include/LIVMapper.h:16, from /home/lixing/fast_livo2/src/fast-livo2-native/src/LIVMapper.cpp:13: /home/lixing/fast_livo2/src/fast-livo2-native/include/IMU_Processing.h:52:12: error: field ‘fout_imu’ has incomplete type ‘std::ofstream’ {aka ‘std::basic_ofstream<char>’} 52 | ofstream fout_imu; | ^~~~~~~~ In file included from /usr/include/c++/9/ios:38, from /usr/include/c++/9/istream:38, from /usr/include/c++/9/sstream:38, from /usr/include/c++/9/complex:45, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/Dense:1, from /usr/include/eigen3/Eigen/Eigen:1, from /home/lixing/fast_livo2/src/fast-livo2-native/include/IMU_Processing.h:16, from /home/lixing/fast_livo2/src/fast-livo2-native/include/LIVMapper.h:16, from /home/lixing/fast_livo2/src/fast-livo2-native/src/LIVMapper.cpp:13: /usr/include/c++/9/iosfwd:119:11: note: declaration of ‘std::ofstream’ {aka ‘class std::basic_ofstream<char>’} 119 | class basic_ofstream; | ^~~~~~~~~~~~~~ make[2]: *** [fast-livo2-native/CMakeFiles/imu_proc.dir/build.make:63:fast-livo2-native/CMakeFiles/imu_proc.dir/src/IMU_Processing.cpp.o] 错误 1 make[1]: *** [CMakeFiles/Makefile2:3562:fast-livo2-native/CMakeFiles/imu_proc.dir/all] 错误 2 make[1]: *** 正在等待未完成的任务.... make[2]: *** [fast-livo2-native/CMakeFiles/laser_mapping.dir/build.make:63:fast-livo2-native/CMakeFiles/laser_mapping.dir/src/LIVMapper.cpp.o] 错误 1 make[1]: *** [CMakeFiles/Makefile2:3589:fast-livo2-native/CMakeFiles/laser_mapping.dir/all] 错误 2 make: *** [Makefile:141:all] 错误 2 Invoking "make -j8 -l8" failed
最新发布
07-22
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值