硬件环境：TX2 ubuntu18.04 Ros melodic
读取串口：ttyTHS2 （tx2自带，TTL电平）
IMU设备：HWT101，（单轴imu，可以提供比较精确的航向信息）
imu数据格式比较固定，原始数据为16进制编码，转换完成之后为float数据类型，本意是要写一个节点，来读取imu数据，并且发布给其他节点使用，第一个是思路是自定义msg消息，来实现发布（后续实现）。本文采用ros自带的msgs中的Float32MultiArray这个向量来进行发布。
直接上代码：imu_reader.cpp
#include <ros/ros.h> #include <serial/serial.h> //Ros already built-in serial port package #include <std_msgs/String.h> #include <std_msgs/Empty.h> #include <std_msgs/Float32MultiArray.h> #define LEN 11 //length of one frame #define MAXSIZE (512*LEN) serial::Serial ser; //declare the serial port object struct IMUdata // imu structures { float accl[3]; float gyro[3]; float angle[3]; float mean_gyro[3]; float tempT; }; // Quene operation:1 init;2:add data:enquene();3,grab data:dequene();4:task end:destory quene! typedef char QelemType; typedef struct { QelemType *base; int front; int rear; }squene; void Initquene(squene &s) { s.base=(QelemType *)malloc(MAXSIZE*sizeof(QelemType)); if(!s.base) exit(-1); s.front=0; s.rear=0; } void enquene(squene &s,QelemType e) { if(((s.rear+1)%MAXSIZE)==s.front) { printf("Quene is Full!\n"); exit(-1); } s.base[s.rear]=e; s.rear=(s.rear+1)%MAXSIZE; } void dequene(squene &s,QelemType &e) { if(s.rear==s.front) { printf("Quene is Empty!\n"); exit(-1); } e=s.base[s.front]; s.front=(s.front+1)%MAXSIZE; } int getQlength(squene s) { return (s.rear-s.front+MAXSIZE)%MAXSIZE; } void destory(squene &s) { free(s.base); s.base=NULL; }
char check_and(char *writebuf, int len) { int i =0; char sum=0; for (i=0;i<len;i++) { sum+=writebuf[i]; } return sum; } //回调函数 //void write_callback(const std_msgs::String::ConstPtr& msg) //{ // ROS_INFO_STREAM("Writing to serial port" <<msg->data); // ser.write(msg->data); //发送串口数据 //} int main (int argc, char** argv) { ros::init(argc, argv, "imu_reader"); //Initialize node ros::NodeHandle nh; //declare the node-handle //ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback); //Subscriber other node's msg //ros::Publisher read_pub = nh.advertise<std_msgs::String>("IMU_data", 1000); //Publisher msgtype("name",(int)len of quene) ros::Publisher read_pub = nh.advertise<std_msgs::Float32MultiArray>("IMU_data", 1000); //imudata publish by flaot_array try { ser.setPort("/dev/ttyTHS2"); ser.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(50); ser.setTimeout(to); ser.open(); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port "); return -1; } if(ser.isOpen()) //check port Isopen? { ROS_INFO_STREAM("SerialPort ttyTHS2 initialized"); } else { return -1; } char revBuf2[LEN]; IMUdata imudata; squene Qs; int len; //the len of Qs Initquene(Qs); //must initial Qs, otherwise segment dault ros::Rate loop_rate(50); // set loop_rate,unit:Hz while(ros::ok()) { //ser.write(sendbuf,9); //tranmit data:ser.write(uint8_t *buf,size_t len) uint8_t buff[1024]={0}; size_t n=ser.available(); if(n!=0) { printf("reci: "); n=ser.read(buff,n); for(int i=0;i<n;i++) { printf(" %x",buff[i]); enquene(Qs,buff[i]); // put revi_data into Qs } printf("\n"); } len=getQlength(Qs); // get len of current Qs while(len>=LEN) { if(Qs.base[Qs.front]==0x55) { memset(revBuf2, 0, sizeof(revBuf2)); for(int i=0;i<LEN;i++) { dequene(Qs,revBuf2[i]); } if(revBuf2[0]==0x55 && revBuf2[1]==0x51 && revBuf2[10]==(check_and(revBuf2,10))) { imudata.accl[0]=(short(revBuf2[3]<<8)|revBuf2[2])/32768.0*16; imudata.accl[1]=(short(revBuf2[5]<<8)|revBuf2[4])/32768.0*16; imudata.accl[2]=(short(revBuf2[7]<<8)|revBuf2[6])/32768.0*16; imudata.tempT=(short(revBuf2[9]<<8)|revBuf2[8])/100.0; printf("Xaccl=%f ",imudata.accl[0]); printf("Yaccl=%f ",imudata.accl[1]); printf("Zaccl=%f ",imudata.accl[2]); printf("\n"); } else if(revBuf2[0]==0x55 && revBuf2[1]==0x52 && revBuf2[10]==(check_and(revBuf2,10))) { imudata.gyro[0]=(short(revBuf2[3]<<8)|revBuf2[2])/32768.0*2000; imudata.gyro[1]=(short(revBuf2[5]<<8)|revBuf2[4])/32768.0*2000; imudata.gyro[2]=(short(revBuf2[7]<<8)|revBuf2[6])/32768.0*2000; imudata.tempT=(short(revBuf2[9]<<8)|revBuf2[8])/100.0; printf("Xgyro=%f ",imudata.gyro[0]); printf("Ygyro=%f ",imudata.gyro[1]); printf("Zgyro=%f ",imudata.gyro[2]); printf("\n"); } else if(revBuf2[0]==0x55 && revBuf2[1]==0x53 && revBuf2[10]==(check_and(revBuf2,10))) { imudata.angle[0]=(short(revBuf2[3]<<8)|revBuf2[2])/32768.0*180; imudata.angle[1]=(short(revBuf2[5]<<8)|revBuf2[4])/32768.0*180; imudata.angle[2]=(short(revBuf2[7]<<8)|revBuf2[6])/32768.0*180; imudata.tempT=(short(revBuf2[9]<<8)|revBuf2[8])/100.0; printf("Xangle=%f ",imudata.angle[0]); printf("Yangle=%f ",imudata.angle[1]); printf("Zangle=%f ",imudata.angle[2]); printf("\n"); } else { printf("data error!\n"); continue; } } else { char ch; dequene(Qs,ch); printf("choas ASCII=%c\n",ch); } len=getQlength(Qs); } std_msgs::Float32MultiArray myimu; // define myimu data to Float32MultiArray myimu.data.push_back(imudata.accl[0]); // HWT101 not contain accl[3],for future use myimu.data.push_back(imudata.accl[1]); myimu.data.push_back(imudata.accl[2]); myimu.data.push_back(imudata.gyro[0]); myimu.data.push_back(imudata.gyro[1]); myimu.data.push_back(imudata.gyro[2]); myimu.data.push_back(imudata.angle[0]); myimu.data.push_back(imudata.angle[1]); myimu.data.push_back(imudata.angle[2]); read_pub.publish(myimu); //处理ROS的信息，比如订阅消息,并调用回调函数 ros::spinOnce(); loop_rate.sleep(); } }
-------------------源码分割线------------------------------------------
上述cpp中，使用了ros的serial功能包，需要自己先行下载。在串口接收中，使用了队列的数据结构，保证数据的完整性。数据解析根据IMU器件的数据格式，就是一个简单的移位操作。值得注意的是，当声明std_msgs::Float32MultiArray myimu;之后，myimu的data需要使用push_back进行压入，数据顺序储存，并非堆栈。
下面是订阅的节点：imu_acceptor.cpp
#include "ros/ros.h" #include "std_msgs/String.h" #include <std_msgs/Float32MultiArray.h>
struct IMUdata // imu structures { float accl[3]; float gyro[3]; float angle[3]; float mean_gyro[3]; float tempT; }; IMUdata iImu; void imu_acceptorCallback(const std_msgs::Float32MultiArray::ConstPtr& raw_imu) { iImu.accl[0]=raw_imu->data[0]; iImu.accl[1]=raw_imu->data[1]; iImu.accl[2]=raw_imu->data[2]; iImu.gyro[0]=raw_imu->data[3]; iImu.gyro[1]=raw_imu->data[4]; iImu.gyro[2]=raw_imu->data[5]; iImu.angle[0]=raw_imu->data[6]; iImu.angle[1]=raw_imu->data[7]; iImu.angle[2]=raw_imu->data[8]; printf("Xgyro=%f ",iImu.gyro[0]); printf("Ygyro=%f ",iImu.gyro[1]); printf("Zgyro=%f ",iImu.gyro[2]); printf("\n"); printf("Xangle=%f ",iImu.angle[0]); printf("Yangle=%f ",iImu.angle[1]); printf("Zangle=%f ",iImu.angle[2]); printf("\n"); }
int main(int argc, char **argv) { ros::init(argc,argv,"imu_acceptor"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("IMU_data",1000,imu_acceptorCallback); ros::spin(); return 0; }
-------------------------------源码分割线----------------------------------------------
上述中，注意包含头文件#include <std_msgs/Float32MultiArray.h> ，在回调函数之中，读取数据直接进行赋值即可。
在Cmakelist.txt中，
添加依赖的功能包
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs serial )
添加生成可执行文件语句：
add_executable(imu_reader src/imu_reader.cpp) target_link_libraries(imu_reader ${catkin_LIBRARIES})
add_executable(imu_acceptor src/imu_acceptor.cpp) target_link_libraries(imu_acceptor ${catkin_LIBRARIES})
注意上述serial一定要记得添加，如果在新建功能包的时候没有添加，那也只需要在cmakelist.txt文件和package.xml中增加即可。
package.xml文件内容：
<?xml version="1.0"?> <package format="2"> <name>read_imu</name> <version>0.0.0</version> <description>The read_imu package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
<!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Example: --> <!-- <url type="website">http://wiki.ros.org/read_imu</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies --> <!-- Dependencies can be catkin packages or system dependencies --> <!-- Examples: --> <!-- Use depend as a shortcut for packages that are both build and exec dependencies --> <!-- <depend>roscpp</depend> --> <!-- Note that this is equivalent to the following: --> <!-- <build_depend>roscpp</build_depend> --> <!-- <exec_depend>roscpp</exec_depend> --> <!-- Use build_depend for packages you need at compile time: --> <!-- <build_depend>message_generation</build_depend> --> <!-- Use build_export_depend for packages you need in order to build against this package: --> <!-- <build_export_depend>message_generation</build_export_depend> --> <!-- Use buildtool_depend for build tool packages: --> <!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- Use exec_depend for packages you need at runtime: --> <!-- <exec_depend>message_runtime</exec_depend> --> <!-- Use test_depend for packages you need only for testing: --> <!-- <test_depend>gtest</test_depend> --> <!-- Use doc_depend for packages you need only for building documentation: --> <!-- <doc_depend>doxygen</doc_depend> --> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>rospy</build_depend> <build_depend>std_msgs</build_depend> <build_depend>serial</build_depend> <build_export_depend>roscpp</build_export_depend> <build_export_depend>rospy</build_export_depend> <build_export_depend>std_msgs</build_export_depend> <build_export_depend>serial</build_export_depend> <exec_depend>roscpp</exec_depend> <exec_depend>rospy</exec_depend> <exec_depend>std_msgs</exec_depend> <exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <export> <!-- Other tools can request additional information be placed here -->
</export> </package>
----------------------------------------------分割线---------------------------------
功能就实现了，通过这种方式，对于每一种硬件外设，都可以写一个单独的节点，放在ros网络中，ros的通信机制是很强大的。
