底层板子和电脑通讯，需要一种相互之间约定规则，也就是通讯协议。常用的数据协议组成基本：数据头+校验+数据+数据尾。数据头一般包括标志位/数据有效位/数据类型/时间等，校验一般常用的抑或运算，累加求和等，数据尾一般就是数据的结束标志。本文主要是探讨记录，python程序与arduino之间的串口通讯问题。其中上层借鉴ROS的串口协议进行处理实现。
ＲＯＳ通讯协议
主要是为了解决与ROS对接问题，所以研究分析ROSserial 的具体实现，用于借鉴。
协议包格式
1st Byte - Sync Flag (Value: 0xff)
2nd Byte - Sync Flag / Protocol version
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
N Bytes - Serialized Message Data
Byte N+8 - Checksum over Topic ID and Message Data
不同ROS发行版对应不同协议版本字段定义（0xff：ROS Groovy, 0xfe on ROS Hydro, Indigo, and Jade.）。Topics ID 0-100为系统功能专用主题使用, 这些主题类似于消息 rosserial_msgs/TopicInfo 中定义的那些特定主题。长度和data的checksum字段用于确保包的完整性，data的checksum可以按照如下公式计算：
255 - ( (Topic ID Low Byte + Topic ID High Byte + data byte values) % 256)
ROS代码实现：
（SerialClient.py 文档内）
class SerialClient:
{
...
def send(self, topic, msg):
# Send a message on a particular topic to the device.
with self.mutex:
length = len(msg)
if self.buffer_in > 0 and length > self.buffer_in:
rospy.logerr("Message from ROS network dropped: message larger than buffer.")
print msg
return -1
else:
#modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte)
# second byte of header is protocol version
msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 )
msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 )
data = "\xff" + self.protocol_ver + chr(length&255) + chr(length>>8) + chr(msg_len_checksum) + chr(topic&255) + chr(topic>>8)
data = data + msg + chr(msg_checksum)
self.port.write(data)
return length
...
def run(self):
#Forward recieved messages to appropriate publisher.
data = ''
while not rospy.is_shutdown():
if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
if (self.synced == True):
rospy.logerr("Lost sync with device, restarting...")
else:
rospy.logerr("Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino")
self.lastsync_lost = rospy.Time.now()
self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "no sync with device")
self.requestTopics()
self.lastsync = rospy.Time.now()
# This try-block is here because we make multiple calls to read(). Any one of them can throw
# an IOError if there's a serial problem or timeout. In that scenario, a single handler at the
# bottom attempts to reconfigure the topics.
try:
if self.port.inWaiting() < 1:
time.sleep(0.001)
continue
flag = [0,0]
flag[0] = self.tryRead(1)
if (flag[0] != '\xff'):
continue
flag[1] = self.tryRead(1)
if ( flag[1] != self.protocol_ver):
self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
rospy.logerr("Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
protocol_ver_msgs = {'\xff': 'Rev 0 (rosserial 0.4 and earlier)', '\xfe': 'Rev 1 (rosserial 0.5+)', '\xfd': 'Some future rosserial version'}
if (flag[1] in protocol_ver_msgs):
found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]]
else:
found_ver_msg = "Protocol version of client is unrecognized"
rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver]))
continue
msg_len_bytes = self.tryRead(2)
msg_length, = struct.unpack("<h", msg_len_bytes)
msg_len_chk = self.tryRead(1)
msg_len_checksum = sum(map(ord, msg_len_bytes)) + ord(msg_len_chk)
if msg_len_checksum % 256 != 255:
rospy.loginfo("wrong checksum for msg length, length %d" %(msg_length))
rospy.loginfo("chk is %d" % ord(msg_len_chk))
continue
# topic id (2 bytes)
topic_id_header = self.tryRead(2)
topic_id, = struct.unpack("<h", topic_id_header)
try:
msg = self.tryRead(msg_length)
except IOError:
self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Packet Failed : Failed to read msg data")
rospy.loginfo("Packet Failed : Failed to read msg data")
rospy.loginfo("msg len is %d",len(msg))
raise
# checksum for topic id and msg
chk = self.tryRead(1)
checksum = sum(map(ord, topic_id_header) ) + sum(map(ord, msg)) + ord(chk)
if checksum % 256 == 255:
self.synced = True
try:
self.callbacks[topic_id](msg)
except KeyError:
rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
rospy.sleep(0.001)
else:
rospy.loginfo("wrong checksum for topic id and msg")
except IOError:
# One of the read calls had an issue. Just to be safe, request that the client
# reinitialize their topics.
self.requestTopics()
} 上两段代码是从ROS程序中找到的串口生成发送指令，以及读取解析指令的程序。其中
struct.unpack("<h", topic_id_header)
struct.pack()
是python 的一个struct模块，实现对数据进行字节转化解包与打包的作用。
参考博客
在代码中还发现了一下函数，单未找到具体实现方法在哪里，所以记录下，留待后续大神指点。跟人感觉应该实现的功能与struct相同，可能针对数据类型不一样，不过仅仅个人猜测尚未证实。
message.serialize(data)
message.deserialize(data_buffer)
通过struct.pack(fat,vel,vel…)将数据生成为二进制字节流。下位机接受串口字节流数据，根据上位机序列化的数据，将二进制转化生成对应数据。由于下位机采用的Arduino uno ,所以根据支持的数据格式，实际float和double实际上是相同精度和类型,对应的４个字节表示一个float/double类型数据。另外arduino板子采用小端存储，所以上位机转字节流包的时候需要采用小端进行转化。
Arduino 数据类型
需要解决以下问题：
１、４byte与float(double)数据互转
２、１byte 与char 数据
３、２byte 转化为int数据
通过参考资料有两种普遍推荐的，一种是指针强转，一种借助union特性。指针的具体可以参照文献进行尝试，由于未知原因，第一次尝试指针强转时未能成功。现在回头想一下，个人分析感觉应该是数据大小端存储的原因导致无法解析出数据。后续应用了union的方式解决了问题，此处记录我所使用的union方法。
查询c语言union联合体，简单来说就是，基地址相同，不同数据类型根据自己的所占空间（字节）共用一块内存对数据进行存储。根据读取指定方式对内存中存储的数据进行解析。
union <name>
{
<datatype> <1st variable name>;
<datatype> <2nd variable name>;
.
.
.
<datatype> <nth variable name>;
} <union variable name>;数据小端／大端模式存储情况对数据存储的影响。首先定义一个union联合体，然后存入一个２byte的数据，看下具体在实际内存中的存储。具体程序可以参考这篇不错的博文。
a = 0x1234
小端模式
大端模式
对于arduino想存储不同数目的指令，所以需要一个类似vector容器来动态的改变数组的大小。解决方案，有链表，另外也可以添加参考资料12的c++for arduino链接库（又说mega2560一下的可能会出现内存不足的可能，尚未实验）。
参考资料
参考资料１
参考资料２
参考资料３
参考资料４
参考资料５
参考资料６
参考资料７
参考资料８
参考资料９
参考资料10
参考资料11
参考资料12
（数据序列化相关参数，具体协议待补全）
ROSserial 安装
命令对应对应的ros版本进行apt-get方式安装
sudo apt-get install ros-kinetic-serial
测试是否安装成功
roscd serial
安装rosserial-arduino
sudo apt-get install ros-kinetic-rosserial-arduino
安装rosserial
sudo apt-get install ros-kinetic-rosserial
进入arduino IDE安装目录库文件夹~/Arduino/libraries安装对应的库.
rosrun rosserial_arduino make_libraries.py ./
（未完待续）
扫一扫
1007
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
