boost解析io转换成为imu_msgs

29人阅读 评论(0) 收藏 举报
下面使用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++

查看评论

boost::asio编程-域名解析

在网络通信中通常我们并不直接使用IP地址,而是使用域名。这时候我们就需要用reslover类来通过域名获取IP,它可以实现与IP版本无关的网址解析。 #include "stdafx.h" #incl...
  • anda0109
  • anda0109
  • 2014-12-17 14:25:49
  • 3236

(boost::format)io_service和io_service::work的基础使用

io_service和io_service::work的基础使用
  • u013600225
  • u013600225
  • 2015-12-02 20:26:15
  • 897

boost 异步IO 服务端实例代码

/* * tcp_async_echo_server.cpp * * Created on: Dec 16, 2013 3:17:37 PM * Author: Jeffrey C...
  • u011676589
  • u011676589
  • 2013-12-17 17:24:21
  • 1083

boost-io_service的工作方式

io_service在ASIO中有着举足轻重的地位置如果对io_service的实现不感兴趣可以选择性无视这篇文章。在开始说io_service之前,先介绍一下ASIO给我们提供的可用于TCP服务器开...
  • twtydgo
  • twtydgo
  • 2016-03-11 16:16:53
  • 1450

boost库之ptree解析xml

// FirstTest.cpp : 定义控制台应用程序的入口点。 //ptree解析xml #include "stdafx.h" #include #include #include ...
  • Leeboy_Wang
  • Leeboy_Wang
  • 2015-02-28 17:40:35
  • 3770

boost asio 在linux下的proactor异步执行流程分析(一)

前几天在看代码的时候上网上网找了关于异步proactor 执行流程的分析,在windows下面到有几篇不错的博文,但是在linux下面几乎没有很详细的介绍,现在看完了,写一下自己在看的过程中的疑惑和自...
  • bao_hu_lu
  • bao_hu_lu
  • 2015-06-13 15:25:12
  • 648

使用boost 完成字节序转换

boost asio 库封装了各平台下的网络函数,用如下指令包含asio库: #include 现在可以使用ntohs ntohl  htons htonl等网络字节转换库了。 也可以使用bo...
  • gxp
  • gxp
  • 2012-02-10 10:38:50
  • 2795

boost::ASIO的同步方式和异步方式

http://blog.csdn.net/zhuky/article/details/5364574 http://blog.csdn.net/zhuky/article/details/53646...
  • damonxx
  • damonxx
  • 2016-09-13 09:31:33
  • 3725

boost 分析命令行参数

#include #include #include using namespace std; using namespace boost::program_options; int main...
  • wangxvfeng101
  • wangxvfeng101
  • 2013-09-24 16:00:43
  • 2844

boost::io_service解读

boost::io_service的队列模型、线程模型、源码解读。
  • ybxuwei
  • ybxuwei
  • 2016-11-26 16:28:36
  • 3791
    个人资料
    等级:
    访问量: 7万+
    积分: 1389
    排名: 3万+
    最新评论