RoboMaster EP明文SDK读取赛事键盘数据并控制底盘运动

目录

一、硬件：

二、简介

控制流程：

三、源码

robot_connection.py

usb_connection.py

---

一、硬件：

        RoboMaster EP、jetson nano、赛事服务器、操作端电脑

二、简介

        RoboMaster EP和jetson nano用一根USB Micro连接，使用明文SDK读取赛事键盘数据，并以此控制机器人运动。

       RoboMaster 开发者页面：欢迎来到 RoboMaster 开发者页面 — RoboMaster Developer Guide 文档

      

如上图，需要使用socket与EP建立两个连接：

        一：控制命令，IP：192.168.42.2，端口：40923，连接方式TCP，所有发送给EP的指令都通过该连接发送。

        二：消息推送，IP：192.168.42.2，端口：40924，连接方式UDP，用于接收赛事数据。

控制流程：

1、建立上述两个连接

2、使用控制命令端口发送'command'进入SDK模式

3、使用控制命令端口发送'game_msg on'打开赛事数据推送

4、使用消息推送端口接收数据并解码

5、使用解码后的数据通过控制命令端口控制底盘运动

三、源码

robot_connection.py

#! python3

import socket
import threading
import select
import queue


class RobotConnection(object):
    """
    Create a RobotConnection object with a given robot ip.
    """
    VIDEO_PORT = 40921
    AUDIO_PORT = 40922
    CTRL_PORT = 40923
    PUSH_PORT = 40924
    EVENT_PORT = 40925
    IP_PORT = 40926

    def __init__(self, robot_ip=''):
        self.robot_ip = robot_ip

        self.video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.audio_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.ctrl_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.push_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.ip_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        self.push_socket.bind(('', RobotConnection.PUSH_PORT))
        self.ip_socket.bind(('', RobotConnection.IP_PORT))

        self.cmd_socket_list = [self.ctrl_socket, self.push_socket, self.event_socket]
        self.cmd_socket_msg_queue = {
            self.video_socket: queue.Queue(32),
            self.audio_socket: queue.Queue(32),
            self.ctrl_socket: queue.Queue(16),
            self.push_socket: queue.Queue(16),
            self.event_socket: queue.Queue(16)
        }
        self.cmd_socket_recv_thread = threading.Thread(target=self.__socket_recv_task)

        self.is_shutdown = True

    def update_robot_ip(self, robot_ip):
        """
        Update the robot ip
        """
        self.robot_ip = robot_ip

    def get_robot_ip(self, timeout=None):
        """
        Get the robot ip from ip broadcat port

        If optional arg 'timeout' is None (the default), block if necessary until
        get robot ip from broadcast port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and return None if no data back from
        robot broadcast port within the time. Otherwise, return the robot ip 
        immediately.
        """
        self.ip_socket.settimeout(timeout)
        msg = None
        try:
            msg, addr = self.ip_socket.recvfrom(1024)
        except Exception as e:
            print('Get robot ip failed, please check the robot networking-mode and connection !')
        else:
            msg = msg.decode('utf-8')
            msg = msg[msg.find('robot ip ') + len('robot ip ') : ]

        return msg

    def open(self): 
        """
        Open the connection

        It will connect the control port and event port with TCP and start a data
        receive thread.
        """
        self.ctrl_socket.settimeout(5)

        try:
            self.ctrl_socket.connect((self.robot_ip, RobotConnection.CTRL_PORT))
            self.event_socket.connect((self.robot_ip, RobotConnection.EVENT_PORT))
        except Exception as e:
            print('Connection failed, the reason is %s'%e)
            return False
        else:
            self.is_shutdown = False
            self.cmd_socket_recv_thread.start() 
            print('Connection successful')
            return True

    def close(self): 
        """
        Close the connection
        """
        self.is_shutdown = True
        self.cmd_socket_recv_thread.join()

    def start_video_recv(self):
        assert not self.is_shutdown, 'CONNECTION INVALID'
        if self.video_socket not in self.cmd_socket_list:
            self.video_socket.settimeout(5)
            try:
                self.video_socket.connect((self.robot_ip, RobotConnection.VIDEO_PORT))
            except Exception as e:
                print('Connection failed, the reason is %s'%e)
                return False 
            self.cmd_socket_list.append(self.video_socket)
        return True

    def stop_video_recv(self):
        if self.video_socket in self.cmd_socket_list:
            self.cmd_socket_list.remove(self.video_socket)
        return True

    def start_audio_recv(self):
        assert not self.is_shutdown, 'CONNECTION INVALID'
        if self.audio_socket not in self.cmd_socket_list:
            self.audio_socket.settimeout(5)
            try:
                self.audio_socket.connect((self.robot_ip, RobotConnection.AUDIO_PORT))
            except Exception as e:
                print('Connection failed, the reason is %s'%e)
                return False
            self.cmd_socket_list.append(self.audio_socket)
        return True

    def stop_audio_recv(self):
        if self.audio_socket in self.cmd_socket_list:
            self.cmd_socket_list.remove(self.audio_socket)
        return True

    def send_data(self, msg):
        """
        Send data to control port
        """
        msg += ';'
        self.__send_data(self.ctrl_socket, msg)

    def recv_video_data(self, timeout=None, latest_data=False):
        """
        Receive control data

        If optional arg 'timeout' is None (the default), block if necessary until
        get data from control port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and reuturn None if no data back from
        robot video port within the time. Otherwise, return the data immediately.
 
        If optional arg 'latest_data' is set to True, it will return the latest
        data, instead of the data in queue tail.

        """
        return self.__recv_data(self.video_socket, timeout, latest_data)

    def recv_audio_data(self, timeout=None, latest_data=False):
        """
        Receive control data

        If optional arg 'timeout' is None (the default), block if necessary until
        get data from control port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and reuturn None if no data back from
        robot video port within the time. Otherwise, return the data immediately.
 
        If optional arg 'latest_data' is set to True, it will return the latest
        data, instead of the data in queue tail.

        """
        return self.__recv_data(self.audio_socket, timeout, latest_data)

    def recv_ctrl_data(self, timeout=None, latest_data=False):
        """
        Receive control data

        If optional arg 'timeout' is None (the default), block if necessary until
        get data from control port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and reuturn None if no data back from
        robot control port within the time. Otherwise, return the data immediately.
 
        If optional arg 'latest_data' is set to True, it will return the latest
        data, instead of the data in queue tail.

        """
        return self.__recv_data(self.ctrl_socket, timeout, latest_data)

    def recv_push_data(self, timeout=None, latest_data=False):
        """
        Receive push data

        If optional arg 'timeout' is None (the default), block if necessary until
        get data from push port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and reuturn None if no data back from
        robot push port within the time. Otherwise, return the data immediately.
 
        If optional arg 'latest_data' is set to True, it will return the latest
        data, instead of the data in queue tail.
        """
        return self.__recv_data(self.push_socket, timeout, latest_data)

    def recv_event_data(self, timeout=None, latest_data=False):
        """
        Receive event data

        If optional arg 'timeout' is None (the default), block if necessary until
        get data from event port. If 'timeout' is a non-negative number,
        it blocks at most 'timeout' seconds and reuturn None if no data back from
        robot event port within the time. Otherwise, return the data immediately.
 
        If optional arg 'latest_data' is set to True, it will return the latest
        data, instead of the data in queue tail.
        """
        return self.__recv_data(self.event_socket, timeout, latest_data)

    def __send_data(self, socket_obj, data):
        assert not self.is_shutdown, 'CONECTION INVALID'
        return socket_obj.send(data.encode('utf-8'))
        
    def __recv_data(self, socket_obj, timeout, latest_data):
        assert not self.is_shutdown, 'CONECTION INVALID'
        msg = None
        if latest_data:
            while self.cmd_socket_msg_queue[socket_obj].qsize() > 1:
                self.cmd_socket_msg_queue[socket_obj].get()
        try:
            msg = self.cmd_socket_msg_queue[socket_obj].get(timeout=timeout)
        except Exception as e:
            return None
        else:
            return msg
        
    def __socket_recv_task(self):
        while not self.is_shutdown:
            rlist, _, _  = select.select(self.cmd_socket_list, [], [], 2)

            for s in rlist:
                msg, addr = s.recvfrom(4096)
                if self.cmd_socket_msg_queue[s].full():
                    self.cmd_socket_msg_queue[s].get()
                self.cmd_socket_msg_queue[s].put(msg)

        for s in self.cmd_socket_list:
            try:
                s.shutdown(socket.SHUT_RDWR)
            except Exception as e:
                pass


def test():
    """
    Test funciton

    Connect robot and query the version 
    """
    robot = RobotConnection('192.168.42.2')
    robot.open()

    robot.send_data('command')
    print('send data to robot   : command')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot.send_data('version ?')
    print('send data to robot   : version ?')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot.send_data('stream on')
    print('send data to robot   : stream on')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)
    result = robot.start_video_recv()
    if result:
        stream_data = robot.recv_video_data(5)
        print('recv video data from robot %s'%stream_data)
        robot.stop_video_recv()
    robot.send_data('stream off')
    print('send data to robot   : stream off')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot.send_data('audio on')
    print('send data to robot   : audio on')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)
    result = robot.start_audio_recv()
    if result:
        stream_data = robot.recv_audio_data(5)
        print('recv audio data from robot %s'%stream_data)
        robot.stop_audio_recv()
    robot.send_data('audio off')
    print('send data to robot   : audio off')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot.send_data('quit')
    print('send data to robot   : quit')
    recv = robot.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot.close()


if __name__ == '__main__':
    test()

usb_connection.py

#! python3

import robot_connection
import time

import threading
USB_DIRECT_CONNECTION_IP = '192.168.42.2'
robot_conn = robot_connection.RobotConnection(USB_DIRECT_CONNECTION_IP)

def send_cmd(cmd:str):
    robot_conn.send_data(cmd)
    # print('tx2 ----> robot   :',cmd)
    recv = robot_conn.recv_ctrl_data(5)
    # print('robot ----> tx2   : %s'%recv)

def main():
    time.sleep(0.05)
    if not robot_conn.open():
        print('open fail')
        exit(1)
    
    send_cmd('command')#进入明文SDK模式

    send_cmd('game_msg on')#打开赛事数据推送

    speed_x_const = 0.3
    speed_y_const = 0.3
    speed_z_const = 10


    speed_x = 0
    speed_y = 0
    speed_z = 0


    while True:
        recv = robot_conn.recv_push_data()               #接收数据
     
        recv_str = str(recv.decode())                    #转为string
        recv_str = recv_str.split('[')[-1].split(']')[0] #截取[]中内容
        recv_str = '[' + recv_str + ']'                  #加上[]
        recv_list = eval(recv_str)                       #转为list
        # print(recv_list)

        cmd_id      = recv_list[0]       #解码
        len         = recv_list[1]
        mouse_press = recv_list[2]
        mouse_x     = recv_list[3]
        mouse_y     = recv_list[4]
        seq         = recv_list[5]
        key_num     = recv_list[6]
        # key1 = recv_list[7] 
        for i in range(key_num):              #多按键处理
            key1 = recv_list[7 + i]
            if key1 == 87:   #W还有一个键值没写  或者可以写成 key1=='w' or key1=='W'没试过，不知道好不好使，其他三个按键类似
                speed_x = speed_x_const
            elif key1 == 83: #S
                speed_x = -speed_x_const
            elif key1 == 65: #A
                speed_y = speed_y_const
            elif key1 == 68: #D
                speed_y = -speed_y_const
            
        if mouse_x > 250:
            speed_z = speed_z_const * (mouse_x - 255)
        elif mouse_x < 5:
            speed_z = speed_z_const * mouse_x
        cmd_send = 'chassis speed x '+str(speed_x)+' y '+str(speed_y)+' z '+ str(speed_z) #明文指令生成
        send_cmd(cmd_send)  #指令发送
        #解析结果打印
        print("cmd:",cmd_id,"len",len,"mousepress",mouse_press,"x:",mouse_x,"y",mouse_y,"seq:",seq,"keynum",key_num,"key1",key1)

try :
    main()
except KeyboardInterrupt:
    robot_conn.send_data('quit')            #推出明文SDK模式
    print('send data to robot   : quit')
    recv = robot_conn.recv_ctrl_data(5)
    print('recv data from robot : %s'%recv)

    robot_conn.close()                      #关闭连接
    print(" Program closed! ")
    pass