ROS中rosserial通讯协议初探

ROS中rosserial通讯协议初探

串行的通讯，我们用串口模拟下通讯图

官方

http://wiki.ros.org/rosserial
rosserial
 

1概述

标准ROS序列化message的协议，可以让一个字符设备（单片机）通过串口或者网口就能实现多topics和services的功能。
1.1客户端库很多
1.2ROS端可以python也可以c++
1.3举例

2限制

2.1Message 最大size ，发布和订阅数量
AVR Model | Input/Output buffer sizes | Publishers/Subscribers

ATMEGA168 | 150/150 bytes | 6/6
ATMEGA328P | 280/280 bytes | 25/25
All others | 512/512 bytes | 25/25
可以自定义大小
2.2~2.4
float string array的数类型

3协议

协议就是在往串口发送的数据的基础上加上包头包尾，
并允许多个topics同时共享一个串口

3.1Packet Format

  1st Byte - Sync Flag (Value: 0xff)
  2nd Byte - Sync Flag / Protocol version  0xff on ROS Groovy, 0xfe on ROS Hydro, Indigo, and Jade.
  3rd Byte - Message Length (N) - Low Byte
  4th Byte - Message Length (N) - High Byte
  5th Byte - Checksum over message length
  6th Byte - Topic ID - Low Byte
  7th Byte - Topic ID - High Byte
  x Bytes  - Serialized Message Data
  Byte x+1 - Checksum over Topic ID and Message Data

横过来看
起始1  /  版本号1  /  内容长度L、H 2  /  内容长度校验1  / Topic ID L、H 2  /  内容x  /   内容校验1

校验算法
 

Message Length Checksum = 255 - ((Message Length High Byte + 
                                   Message Length Low Byte) % 256 )

Message Data Checksum = 255 - ((Topic ID Low Byte +
                                Topic ID High Byte + 
                                Data byte values) % 256)

由此校验算法上可见 Topic ID两个字节是作为校验数据的一部分，数据包长度这两个字节不算在内。

说Topics ID 0-100 系统保留 

如http://docs.ros.org/api/rosserial_msgs/html/msg/TopicInfo.html

uint16 ID_PUBLISHER=0
uint16 ID_SUBSCRIBER=1
uint16 ID_SERVICE_SERVER=2
uint16 ID_SERVICE_CLIENT=4
uint16 ID_PARAMETER_REQUEST=6
uint16 ID_LOG=7
uint16 ID_TIME=10
uint16 ID_TX_STOP=11
uint16 topic_id
string topic_name
string message_type
string md5sum
int32 buffer_size

此处的Topic ID的称谓值得注意，我觉得理解为 Topic类别ID 更好，他区分了这个数据包话题是发布、订阅或service服务端客户端等等

3.2话题协商 Topic Negotiation

在开始数据传输之前，上位机必须向嵌入式设备查询将要发布或订阅的主题的名称和类型。
主题协商包括对主题的查询、对主题数量的响应以及定义每个主题的数据包。对主题的请求使用主题ID 0。

主题请求时 Topic ID设置0

开始/版本号/长度/长度校验/Topic ID/内容校验

0xFF/0xFE/0x00/0x00/0xFF/0x00/0x00/0xFF

会话结束 topic_id = ID_TX_STOP=11（0x0B）

开始/版本号/长度/长度校验/Topic ID/内容校验

0xFF/0xFE/0x00/0x00/0xFF/0x0B/0x00/0xE3

上位机发过来的时钟/同步帧,（ID_TIME=10）
0xFF/0xFE/0x08/0x00/CHK/0x0A/0x00/8个bytes/CHK

当上位机发送主题请求后
0xff 0xfe 0x00 0x00 0xff 0x00 0x00 0xff
下位机应该发送
一系列响应包（消息类型rosserial-msgs/TopicInfo，每个包都包含有关特定主题的信息，用以下数据代替序列化的消息：

uint16 topic_id
  string topic_name
  string message_type
  string md5sum
  int32 buffer_size
注意这里又有一个topic_id，这个topic_id是运行程序中类实例的id，不能与 Topic ID混淆。
这五个数据合在一起是数据包的内容部分。

用单片机实现一个publisher，下面是实验数据分析

[20:46:35.143]发→◇FF FE 00 00 FF 00 00 FF □
[20:46:35.399]收←◆
FF FE 08 00 F7 
0A 00 ----------------------------0x000A=Topic ID  (ID_TIME=10)
00 00 00 00 00 00 00 00 
F5 --------------------------------0xf5包校验0xff-((0x0A+0x00+0+0+0+0+0+0+0+0)%0x100)

FF FE 48 00 B7 -------------------0x0048=72 包数据长度 0xb7=0xff-((0x48+0x00)%0x100)
00 00 -----------------------------0x0000= Topic ID(ID_PUBLISHER=0)
7D 00 -----------------------------0x7D=125 topic_id (publisher ID)
07 00 00 00 ----------------------0x0007=7，数据长度 topic_name="chatter"
63 68 61 74 74 65 72 
0F 00 00 00 ----------------------0x000f=15，数据长度 message_type ="std_msgs/String"
73 74 64 5F 6D 73 67 73 2F 53 
74 72 69 6E 67 
20 00 00 00 ----------------------0x0020=32，数据长度 md5sum="992ce8a1687cec8c8bd883ec73ca41d1"
39 39 32 63 65 38 61 31 36 38 
37 63 65 63 38 63 38 62 64 38 
38 33 65 63 37 33 63 61 34 31 
64 31 
18 01 00 00 ----------------------0x0118=280,是atmage328p的buffer_size  (看2.1)
0C --------------------------------0x0C 包校验

[20:46:36.404]收←◆
FF FE 10 00 EF 
7D 00 
0C 00 00 00 
68 65 6C 6C 6F 20 77 6F 72 6C 64 21 
F9 

[20:46:37.404]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:38.404]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 

FF FE 08 00 F7 
0A 00 
00 00 00 00 00 00 00 00 F5 

[20:46:39.402]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:40.403]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:41.402]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 FF FE 08 00 F7 0A 00 00 00 00 00 00 00 00 00 F5 
[20:46:42.402]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:43.401]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:44.401]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 FF FE 08 00 F7 0A 00 00 00 00 00 00 00 00 00 F5 
[20:46:45.401]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 
[20:46:46.400]收←◆FF FE 10 00 EF 7D 00 0C 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 F9 

上个标记好的图吧

消息类型MD5的对应举例

https://github.com/frankjoshua/rosserial_arduino_lib/blob/master/src/std_msgs/Byte.h

"std_msgs/Byte"
MD5("byte data")="ad736a2e8818154c487bb80fe42ce43b"
"std_msgs/Bool"
MD5("bool data")="8b94c1b53db61fb6aed406028ad6332a"
"std_msgs/String"
MD5("string data")="992ce8a1687cec8c8bd883ec73ca41d1"

关于publisher的ID
https://github.com/frankjoshua/rosserial_arduino_lib/blob/master/src/ros/node_handle.h

void negotiateTopics()
      {
        rosserial_msgs::TopicInfo ti;
        int i;
    
        for(i = 0; i < MAX_PUBLISHERS; i++)
        {
          if(publishers[i] != 0) // non-empty slot
          {
        ReadBuffer buffer;
        
        ti.topic_id = publishers[i]->id_;
        ti.topic_name = (char *) buffer.readTopicName( publishers[i] );
        ti.message_type = (char *) buffer.readMsgInfo( publishers[i]->msg_->getType() );
        ti.md5sum = (char *) buffer.readMsgInfo( publishers[i]->msg_->getMD5() );
        ti.buffer_size = OUTPUT_SIZE;
        publish( publishers[i]->getEndpointType(), &ti );
....

3.3时间

通过向每个方向发送std_msg::Time来处理时间同步。嵌入式设备可以通过发送空时间消息从PC/平板电脑请求当前时间。返回的时间用于查找时钟偏移量。

4 Report a Bug

略

arduino测试源码

#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt16.h>
ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
std_msgs::String str_msg2;
ros::Publisher chatter2("chatter2", &str_msg2);
 
char hello[13] = "hello world!";
char hello2[13] = "hello hello!";



void servo_cb( const std_msgs::UInt16& cmd_msg){
  //servo.write(cmd_msg.data); //set servo angle, should be from 0-180  
  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
}


ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);


void setup()
{
  pinMode(13, OUTPUT);
  //servo.attach(9); //attach it to pin 9
  nh.initNode();
  nh.advertise(chatter);
  nh.advertise(chatter2);
  nh.subscribe(sub);
}

void loop()
{
  str_msg.data = hello;
  chatter.publish( &str_msg );
  str_msg2.data = hello2;
  chatter2.publish( &str_msg2 );
  nh.spinOnce();
  delay(1000);
}

参考
rosserial_python serial_node.py分析
https://www.cnblogs.com/Montauk/p/7158597.html

rosserial_python serial_node.py分析--补遗
https://www.cnblogs.com/Montauk/p/7161927.html