ROS开发（二）——添加传感器话题(姿态传感器)

发表于

这是一个将姿态传感器数据发布到ROS话题上教程，姿态传感器数据通信格式为UART，数据包括航向角的四元素、角加速度、线加速度

MINI SRC姿态传感器(UART)

1. 传感器数据协议

起始帧 本帧字节数 帧功能 航向角/加速度 解算速度/磁力计 帧校验 帧结束
0XA5 0x5A 0x10 0xA1 0xXX 0xXX 0xXX 0xXX 0xXX 0xAA
0xA5 0x5A 0x16 0xA2 0xXX 0xXX 0xXX 0xXX 0xXX 0xAA
  • 0xXX 先为数据高位，紧接着是低位
  • 数据包括航向角、角加速度、线加速度
  • 航向角的单位为0.1度，即乘以0.1后为数据的角度度数；角加速度、线加速度为ADC测量值，分别对应量程换算（角加速度(2000/65536)、线加速度(4.2*9.81/65536)）

2. 编写发布话题数据包

2.1安装额外ROS依赖包（见一）

  • sudo apt-get install ros-kinetic-rosserial

    2.2ubuntu创建ROS包

    uart_read为包名称，后面为依赖项，由于姿态传感器非常适合sensor_msgs/Imu数据结构所以使用

  • roscreate-pkg uart_read roscpp

    2.3编写CMakeLists.txt

    安装生成的CMakeLists.txt修改

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    cmake_minimum_required(VERSION 2.8.3)
    project(uart_read)
    add_compile_options(-std=c++11)

    find_package(catkin REQUIRED COMPONENTS
      roscpp
      serial
      std_msgs
      sensor_msgs
      tf
      geometry_msgs
    )

    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES uart_read
     CATKIN_DEPENDS serial std_msgs sensor_msgs geometry_msgs tf roscpp
    #  DEPENDS system_lib
    )

    # include
      ${catkin_INCLUDE_DIRS}
    )

    add_executable(${PROJECT_NAME}_node src/uart_read.cpp)

    target_link_libraries(${PROJECT_NAME}_node
      ${catkin_LIBRARIES}
    )

2.4修改package.xml

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
<?xml version="1.0"?> <!-- 删除　version=2.0因为版本问题 -->
<package>
  <name>uart_read</name>
  <version>0.0.0</version>
  <description>The uart_read package</description>
  <maintainer email="shisanchuan@todo.todo">shisanchuan</maintainer>
  <license>TODO</license>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>serial</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>serial</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>imu_tools</run_depend>
  <run_depend>tf</run_depend>
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
  </export>
</package>

2.5编写文件src/uart_read.cpp（原生成.cpp文件修改名称得到）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
#include <ros/ros.h>
serial::Serial ros_ser;

//回调函数
void callback(const std_msgs::String::ConstPtr& msg){
     ROS_INFO_STREAM("Write to serial port" << msg->data);
     ros_ser.write(msg->data);
 }
int main (int argc, char** argv){
    std::string port;
    std::string NodeHandle_name;
    std::string pub_name;

     ros::init(argc, argv, "my_serial_node");
     ros::NodeHandle private_node_handle("~");
     private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0");
     private_node_handle.param<std::string>("NodeHandle_name", NodeHandle_name, "imu");
     private_node_handle.param<std::string>("pub_name", pub_name, "data");

     ros::NodeHandle n(NodeHandle_name);
     //订阅主题command
     ros::Subscriber command_sub = n.subscribe("command", 1000, callback);
     //发布主题sensor
     ros::Publisher IMU_pub=n.advertise<sensor_msgs::Imu>(pub_name, 50);
    
    tf::Quaternion orientation;
    tf::Quaternion zero_orientation=tf::createQuaternionFromRPY(0.0,0.0,0.0);

     static tf::TransformBroadcaster tf_br;
     tf::Transform transform;
     transform.setOrigin(tf::Vector3(0,0,0));
backpoint:
     try
     {
         ros_ser.setPort(port);
         ros_ser.setBaudrate(115200);
         ros_ser.open();
     }
     catch (serial::IOException& e)
     {
         ROS_ERROR_STREAM("Unable to open port ");
         return -1;
     }
     if(ros_ser.isOpen()){
         ROS_INFO_STREAM("Serial Port opened");
     }else{
         return -1;
     }
     ros::Rate loop_rate(10);
     sensor_msgs::Imu imu;
     std::string read,input;
     int start_flag;
     while(ros::ok()){
         ros::spinOnce();
         if(ros_ser.available()){
             // std_msgs::String serial_data;
             //获取串口数据
             try{
             read = ros_ser.read(ros_ser.available());
             }
             catch (serial::IOException& e){
                ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection.");
                ros::Duration(5).sleep();
                goto backpoint;//串口不稳定的话，休眠５秒后在次重新尝试连接
            	}
             // ROS_DEBUG("Read size:" << read.size());
             input+=read;
             while(input.length()>=44){//总长度为0x10+0x16
                start_flag=input.find(0xA5);
                 if(start_flag!=std::string::npos){//
                    if(input[start_flag+1]==0x5A&&(unsigned char)input[start_flag+(unsigned char)input[start_flag+2]+1]==0xAA){
                        //is a data
                        if((unsigned char)input[start_flag+3]==0xA1){
                            int16_t yaw,pitch,roll,alt,tempr,press,IMUpersec;
                            yaw       =((0xff&(unsigned char)input[start_flag+4])<<8)|(0xff&(unsigned char)input[start_flag+5]);
                            pitch     =((0xff&(unsigned char)input[start_flag+6])<<8)|(0xff&(unsigned char)input[start_flag+7]);
                            roll      =((0xff&(unsigned char)input[start_flag+8])<<8)|(0xff&(unsigned char)input[start_flag+9]);
                            alt       =((0xff&(unsigned char)input[start_flag+10])<<8)|(0xff&(unsigned char)input[start_flag+11]);
                            tempr     =((0xff&(unsigned char)input[start_flag+12])<<8)|(0xff&(unsigned char)input[start_flag+13]);
                            press     =((0xff&(unsigned char)input[start_flag+14])<<8)|(0xff&(unsigned char)input[start_flag+15]);
                            IMUpersec =((0xff&(unsigned char)input[start_flag+16])<<8)|(0xff&(unsigned char)input[start_flag+17]);

                            if(yaw&0x8000)yaw     =-(yaw&0x7FFF);
                            if(pitch&0x8000)pitch =-(pitch&0x7FFF);
                            if(roll&0x8000)roll   =-(roll&0x7FFF);
							//计算航向角对应的四元素
                            tf::Quaternion orientation=tf::createQuaternionFromRPY(roll*0.1*M_PI/180.0, pitch*0.1*M_PI/180.0, yaw*0.1*M_PI/180.0);
                            tf::Quaternion differential_rotation=zero_orientation.inverse()*orientation;
                            quaternionTFToMsg(differential_rotation, imu.orientation);
                            ros::Time measurement_time = ros::Time::now() + ros::Duration(0.0);
                            transform.setRotation(differential_rotation);
                            tf_br.sendTransform(tf::StampedTransform(transform, measurement_time, "imu_base", "imu_link"));
                            ROS_DEBUG_STREAM("roll"<<roll*0.1<<"  pitch"<<pitch*0.1<<"  yaw"<<yaw*0.1<<"  tem"<<tempr*0.1);

                            input.erase(0,start_flag+input[start_flag+2]+1);
                        }else if((unsigned char)input[start_flag+3]==0xA2){
                            int16_t ax,ay,az,gx,gy,gz,hx,hy,hz;
                            ax =((0xff&(unsigned char)input[start_flag+4])<<8)|(0xff&(unsigned char)input[start_flag+5]);
                            ay =((0xff&(unsigned char)input[start_flag+6])<<8)|(0xff&(unsigned char)input[start_flag+7]);
                            az =((0xff&(unsigned char)input[start_flag+8])<<8)|(0xff&(unsigned char)input[start_flag+9]);
                            gx =((0xff&(unsigned char)input[start_flag+10])<<8)|(0xff&(unsigned char)input[start_flag+11]);
                            gy =((0xff&(unsigned char)input[start_flag+12])<<8)|(0xff&(unsigned char)input[start_flag+13]);
                            gz =((0xff&(unsigned char)input[start_flag+14])<<8)|(0xff&(unsigned char)input[start_flag+15]);
                            hx =((0xff&(unsigned char)input[start_flag+16])<<8)|(0xff&(unsigned char)input[start_flag+17]);
                            hy =((0xff&(unsigned char)input[start_flag+18])<<8)|(0xff&(unsigned char)input[start_flag+19]);
                            hz =((0xff&(unsigned char)input[start_flag+20])<<8)|(0xff&(unsigned char)input[start_flag+21]);

                            if(ax&0x8000)ax     =-(ax&0x7FFF);
                            if(ay&0x8000)ay     =-(ay&0x7FFF);
                            if(az&0x8000)az     =-(az&0x7FFF);
                            if(gx&0x8000)gx     =-(gx&0x7FFF);
                            if(gy&0x8000)gy     =-(gy&0x7FFF);
                            if(gz&0x8000)gz     =-(gz&0x7FFF);
                            if(hx&0x8000)hx     =-(hx&0x7FFF);
                            if(hy&0x8000)hy     =-(hy&0x7FFF);
                            if(hz&0x8000)hz     =-(hz&0x7FFF);
                            imu.header.stamp =  ros::Time::now();
                            imu.header.frame_id = "imu_link";
                            imu.angular_velocity.x =gx * (2000.0/65536.0) ;
                            imu.angular_velocity.y =gy * (2000.0/65536.0) ;
                            imu.angular_velocity.z =gz * (2000.0/65536.0) ;
                            imu.linear_acceleration.x = ax *(4.2/65536.0)*9.81;
                            imu.linear_acceleration.y = ay *(4.2/65536.0)*9.81;
                            imu.linear_acceleration.z = az *(4.2/65536.0)*9.81;
                            IMU_pub.publish(imu);
                            input.erase(0,start_flag+input[start_flag+2]+1);
                        }else{//error !!
                            ROS_INFO_STREAM("error data!!!");
                        }
                    }else{
                        if(input.length()>=start_flag+24)
                            input.erase(0,start_flag+1);
                        else
                            input.erase(0,start_flag);
                    }
                 }else{
                    input.clear();
                 }
            }
             //将串口数据发布到主题sensor
             // sensor_pub.publish(serial_data);
         }
         loop_rate.sleep();
     }
 }

2.6编写launch文件

总共使用四个姿态传感器因此使用四个USB口,注意name和value名称不要冲突

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
<launch>
  <node pkg="uart_read" type="uart_read_node" name="uart_read_node0" required="true">
    <param name="port" value="/dev/ttyUSB0"/>
    <param name="NodeHandle_name" value="imu0"/>
    <param name="pub_name" value="data"/>
  </node>
  <node pkg="uart_read" type="uart_read_node" name="uart_read_node1" required="true">
    <param name="port" value="/dev/ttyUSB1"/>
    <param name="NodeHandle_name" value="imu1"/>
    <param name="pub_name" value="data1"/>
  </node>
  <node pkg="uart_read" type="uart_read_node" name="uart_read_node2" required="true">
    <param name="port" value="/dev/ttyUSB2"/>
    <param name="NodeHandle_name" value="imu2"/>
    <param name="pub_name" value="data1"/>
  </node>
  <node pkg="uart_read" type="uart_read_node" name="uart_read_node3" required="true">
    <param name="port" value="/dev/ttyUSB3"/>
    <param name="NodeHandle_name" value="imu3"/>
    <param name="pub_name" value="data1"/>
  </node>
</launch>

3. 固定USBID编号