[TOC]

背景:虽然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建立连接的过程中主要进行了三次握手:

  1. 第一次握手:Client将标志位SYN置1建立新连接,并随机产生一个值seq=x,并其发送给Server,同时Client进入SYN_SENT状态,等待Server确认。(即Client告诉Server说我要连接了)
  2. 第二次握手:Server收到数据包后,通过SYN=1知道Client在请求建立连接,之后把SYN和ACK(都置为1,ack=x+1,并随机产生一个值seq=y,并将其发送给Client以确认连接请求,Server进入SYN_RCVD状态。(即Server回复说同意连接)
  3. 第三次握手:Client收到确认后,检查ack是否为x+1,ACK是否为1,并将ACK置为1,ack=y+1,同时将其发送给Server,Server检查数据包正确则连接建立成功,Client和Server进入ESTABLISHED状态,完成三次握手,随后Client与Server之间可以开始传输数据了。(即二者互相检查数据正确与否,然后建立连接)


断开连接的过程则进行的四次握手:

  1. 第一次挥手:Client向Server发起请求释放连接,置FIN为1。客户端进入终止等待-1阶段。(即Client告诉Server说我要断开连接,你松手)
  2. 第二次挥手:Server接收到Client的TCP报文后,确认了Client想要释放连接,Server进入CLOSE-WAIT阶段,并向Client发送一段TCP报文。Client收到后进入种植等待-2阶段。(即Server确认释放连接)
  3. 第三次挥手:Server做好了释放连接的准备,再次向Client发出一段TCP报文,此时Server进入最后确认阶段。(即Server准备好后正式释放连接)
  4. 第四次挥手:Client收到从Server发出的TCP报文,确认了Server已做好释放连接的准备,于是进入时间等待阶段,并向Server发送一段报文,等待2MSL关闭。(即断开连接)


2. Python实现TCP通讯

首先需要添加一个 Socket 包

import socket

在程序中,如果想要完成一个tcp服务器的功能,需要的流程如下:

  1. 创建一个socket套接字
  2. bind 绑定ip和port
  3. listen使套接字变为可以被动链接
  4. 等待客户端的链接
  5. 接收数据

并且还要考虑到可以一次让多个客户端进行连接,且每个客户端可以循环发送信息,最后还要让客户端可以关闭连接,完整代码如下,都写了比较详细的注释:

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下的调试助手举例:

  1. 选择工作模式为TCP客户端
  2. 输入服务器端的IP和端口(即上方程序的IP端口)
  3. 运行服务端程序,点击打开
  4. 发送信息即可


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互转区别