背景:虽然ROS本身就是一种网络通讯,但是一个项目需要不同的人进行配合,考虑到不同模块的接口对齐,有时基础的TCP和UDP通讯也是一些必要的通讯手段,例如上位机的开发如果需要适配多种环境,此时ROS接口或许就不太适用了,另外也不能要求每一个合作的小伙伴都会ROS开发对吧。此时在机器人的开发过程中,就要留出TCP通讯的接口,本文主要探讨一下Python下如何进行TCP通讯,以及如何通过接收到的数据来调用ROS的接口控制机器人。
1. TCP通讯
TCP协议,传输控制协议(英语:Transmission Control Protocol,缩写为:TCP)是一种面向连接的、可靠的、基于字节流的通信协议
TCP把连接作为最基本的抽象单元,每条TCP连接有两个端点,TCP连接的端点即套接字。
套接字 Socket = (IP地址 + 端口号)
TCP连接={socket1, socket2} = {(IP1 : port1), (IP2, port2)}
TCP提供全双工通信。
TCP建立与断开连接
TCP建立连接的过程中主要进行了三次握手:
- 第一次握手:Client将标志位SYN置1建立新连接,并随机产生一个值seq=x,并其发送给Server,同时Client进入SYN_SENT状态,等待Server确认。(即Client告诉Server说我要连接了)
- 第二次握手:Server收到数据包后,通过SYN=1知道Client在请求建立连接,之后把SYN和ACK(都置为1,ack=x+1,并随机产生一个值seq=y,并将其发送给Client以确认连接请求,Server进入SYN_RCVD状态。(即Server回复说同意连接)
- 第三次握手:Client收到确认后,检查ack是否为x+1,ACK是否为1,并将ACK置为1,ack=y+1,同时将其发送给Server,Server检查数据包正确则连接建立成功,Client和Server进入ESTABLISHED状态,完成三次握手,随后Client与Server之间可以开始传输数据了。(即二者互相检查数据正确与否,然后建立连接)
断开连接的过程则进行的四次握手:
- 第一次挥手:Client向Server发起请求释放连接,置FIN为1。客户端进入终止等待-1阶段。(即Client告诉Server说我要断开连接,你松手)
- 第二次挥手:Server接收到Client的TCP报文后,确认了Client想要释放连接,Server进入CLOSE-WAIT阶段,并向Client发送一段TCP报文。Client收到后进入种植等待-2阶段。(即Server确认释放连接)
- 第三次挥手:Server做好了释放连接的准备,再次向Client发出一段TCP报文,此时Server进入最后确认阶段。(即Server准备好后正式释放连接)
- 第四次挥手:Client收到从Server发出的TCP报文,确认了Server已做好释放连接的准备,于是进入时间等待阶段,并向Server发送一段报文,等待2MSL关闭。(即断开连接)
2. Python实现TCP通讯
首先需要添加一个 Socket 包
import socket
在程序中,如果想要完成一个tcp服务器的功能,需要的流程如下:
- 创建一个socket套接字
- bind 绑定ip和port
- listen使套接字变为可以被动链接
- 等待客户端的链接
- 接收数据
并且还要考虑到可以一次让多个客户端进行连接,且每个客户端可以循环发送信息,最后还要让客户端可以关闭连接,完整代码如下,都写了比较详细的注释:
import socket
# 创建socket
serverSocket = socket(AF_INET, SOCK_STREAM)
serverSocket.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
# 本地信息
address = ('192.168.1.27', 8113)
# 绑定
serverSocket.bind(address)
# 转主动为被动
serverSocket.listen(5)
while True:
# 等待新的客户端连接
TCPsocket, clientInfo = serverSocket.accept()
while True:
# 接收对方发送过来的数据
socket_data = TCPsocket.recv(1024) # 接收1024个字节
if socket_data:
print('接收到数据: ', recv_data.decode('gbk'))
else:
break
TCPsocket.close()
serverSocket.close()
3. 调试TCP服务端
这里推荐一个windows下的调试助手:卓岚TCP/UDP调试工具
Ubuntu下的调试助手:mnetassist(Gitcode镜像)
以windows下的调试助手举例:
- 选择工作模式为TCP客户端
- 输入服务器端的IP和端口(即上方程序的IP端口)
- 运行服务端程序,点击打开
- 发送信息即可
4. 通过TCP控制Originbot小车
首先捋一下逻辑,Originbot的一切控制操作都通过ROS2来进行,我们可以用我们最为熟悉的cmd_vel发送速度指令来入手,定义通讯协议实现前进后退左右转和停止。
4.1 通讯协议
Data | Length(B) | Hex | Result |
---|---|---|---|
Frame Header | 1 | 55 | 0x55 |
Frame Mark | 1 | 02 | 0x02 |
data | n | * | data |
Sum Check | 1 | 0d | 0x0d |
data 格式
说明 | length(2b) | cmd(1b) | data(nb) |
---|---|---|---|
小车停止(T) | 4 | 0x80 | 1 |
小车停止(S) | 4 | 0x00 | 1 |
小车前进(T) | 4 | 0x81 | 2 |
小车前进(S) | 4 | 0x01 | 1 |
小车后退(T) | 4 | 0x82 | 2 |
小车后退(S) | 4 | 0x02 | 2 |
小车右转(T) | 4 | 0x83 | 3 |
小车右转(S) | 4 | 0x03 | 3 |
小车左转(T) | 4 | 0x84 | 4 |
小车左转(S) | 4 | 0x04 | 4 |
即:发送十六进制数据:
55 02 04 00 81 — 小车前进(返回04 00 01 01)
55 02 04 00 82 — 小车后退(返回04 00 02 01)
……
4.2 bytes转hex
其中,针对十六进制数据,通讯时接收到的格式为bytes,bytes转hex一定不要忘记,一定要进行转换,当时就因为这个死活读不到正确的数据,再次特意强调一下。
另外转换这里也有个很大的坑,如果是用ROS1,环境为Python2时,语法为:
socket_data = TCPsocket.recv(1024).encode('hex')
而在ROS2环境为Python3时,因为Python3中,string和bytes的实现发生了重大的变化,这个转换也不能再用encode/decode完成了,此时语法为(Python3.5之后):
socket_data = TCPsocket.recv(1024).hex()
发送指令时,Python2:
TCPsocket.send("04 00 01 01".decode('hex'))
Python3:
TCPsocket.send(bytes.fromhex("04 00 01 01"))
4.3 ROS命令
对于有基础的同学们来说,ROS指令相对就比较容易理解一些了,可以参考Originbot的官方教程:OriginBot智能机器人开源套件-基础功能编程
cmd_vel控制部分的源码如下:
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import Twist # 速度话题的消息
"""
创建一个发布者节点
"""
class PublisherNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.pub = self.create_publisher(Twist, 'cmd_vel', 10) # 创建发布者对象(消息类型、话题名、队列长度)
self.timer = self.create_timer(0.5, self.timer_callback) # 创建一个定时器(单位为秒的周期,定时执行的回调函数)
def timer_callback(self): # 创建定时器周期执行的回调函数
twist = Twist() # 创建一个Twist类型的消息对象
twist.linear.x = 0.2 # 填充消息对象中的线速度
twist.angular.z = 0.8 # 填充消息对象中的角速度
self.pub.publish(twist) # 发布话题消息
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (twist.linear.x, twist.angular.z))
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = PublisherNode("draw_circle") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
4.4 完整代码
对上述代码,加入TCP通讯和通讯协议解析后,就可以根据读到的消息来调用不同的命令函数了,完整代码如下:
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import Twist # 速度话题的消息
from threading import Thread
from socket import *
class PublisherNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.pub = self.create_publisher(Twist, 'cmd_vel', 10) # 创建发布者对象(消息类型、话题名、队列长度)
self.twist = Twist() # 创建一个Twist类型的消息对象
def forward(self):
self.twist.linear.x = 0.3 # 填充消息对象中的线速度
self.twist.angular.z = 0.0 # 填充消息对象中的角速度
self.pub.publish(self.twist) # 发布话题消息
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (self.twist.linear.x, self.twist.angular.z))
def backward(self):
self.twist.linear.x = -0.3
self.twist.angular.z = 0.0
self.pub.publish(self.twist)
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (self.twist.linear.x, self.twist.angular.z))
def turn_left(self):
self.twist.linear.x = 0.0
self.twist.angular.z = -0.3
self.pub.publish(self.twist)
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (self.twist.linear.x, self.twist.angular.z))
def turn_right(self):
self.twist.linear.x = 0.0
self.twist.angular.z = 0.3
self.pub.publish(self.twist)
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (self.twist.linear.x, self.twist.angular.z))
def stop(self):
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
self.pub.publish(self.twist)
self.get_logger().info('Publishing: "linear: %0.2f, angular: %0.2f"' % (self.twist.linear.x, self.twist.angular.z))
# 接受TCP消息
def Receive(self, TCPsocket):
while True:
# socket_data = TCPsocket.recv(1024).encode('hex')
socket_data = TCPsocket.recv(1024).hex() # 转换16进制
self.get_logger().info(socket_data)
if len(socket_data):
key = socket_data
frame_header = key[0:2]
frame_mark = key[2:4]
data_length = key[4:6]
data_cmd = key[6:10]
data_data = key[10:12]
if frame_header+frame_mark == '5502':
self.get_logger().info('接收到数据: '+frame_header + " " + frame_mark + " " + data_length + " " + data_cmd + " " + data_data)
if data_cmd == '0080': # 停止
self.stop()
self.get_logger().info('--------小车停止--------')
# TCPsocket.send("04000001".decode('hex'))
TCPsocket.send(bytes.fromhex("04 00 00 01"))
elif data_cmd == '0081': # 前进
self.forward()
self.get_logger().info('--------小车前进--------')
TCPsocket.send(bytes.fromhex("04 00 01 01"))
elif data_cmd == '0082': # 后退
self.backward()
self.get_logger().info('--------小车后退--------')
TCPsocket.send(bytes.fromhex("04 00 02 01"))
elif data_cmd == '0083': # 右转
self.turn_left()
self.get_logger().info('--------小车右转--------')
TCPsocket.send(bytes.fromhex("04 00 03 01"))
elif data_cmd == '0084': # 左转
self.turn_right()
self.get_logger().info('--------小车左转--------')
TCPsocket.send(bytes.fromhex("04 00 04 01"))
elif data_cmd == '0085': # 小车充电
# self.get_logger().info('-----小车充电-----')
# dock_start()
TCPsocket.send(bytes.fromhex("04 00 05 01"))
# 测试使用
elif key == '88':
self.get_logger().info("-----关闭TCP连接-----")
TCPsocket.close()
break
else:
self.get_logger().info("Error, The Target Message is WRONG!!!")
# break
else:
self.get_logger().info('-----未接收到客户端数据,可能连接已经断开-----')
# 数据中断时进行服务重启程序,先close 再accept等待重新连线
# 可以防止出现当client意外终止导致server的中断(Broken pipe错误)
self.get_logger().info('-----正在重新建立连接-----')
TCPsocket.close()
TCPsocket, clientInfo = serverSocket.accept()
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = PublisherNode("draw_circle") # 创建ROS2节点对象并进行初始化
# 启动TCP通讯
serverSocket = socket(AF_INET, SOCK_STREAM)
serverSocket.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
serverSocket.bind(('192.168.1.27', 8113)) # ×××××这里改IP和端口号××××× #
serverSocket.listen(5)
node.get_logger().info("-----服务器正在启动-----")
TCPsocket, clientInfo = serverSocket.accept()
t1 = Thread(target=node.Receive, args=(TCPsocket,))
t1.start()
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
实现的效果如下(就不上动图了,放一下远程连接的Originbot的终端界面和串口助手的界面)
4.5 延伸
最初这段代码的作用是用来选择目标位置点,由于手头的这个Originbot是视觉版本的,没有slam功能,因此就用简单的运动指令代替了位置点的选择,在这里也希望给大家起到一个抛砖引玉的作用
试想一下,如果解析到的数据为目标点序号,然后对应目标点的坐标,发送给导航程序,不就变成了通过TCP指令来控制机器人的导航了~
参考资料
[1]【TCP通信】原理详解与编程实现(一)
[2] OriginBot智能机器人开源套件—基础功能编程
[3] python3 python2 字符串与hex互转区别
评论(0)
您还未登录,请登录后发表或查看评论