4. 编队控制

4.1. 介绍

编队控制功能是通过明文 SDK 对连接在同一个局域网内的多个机器人进行动作编排,实现整体控制的功能。用户可以使用该功能进行更复杂的动作控制,实现编队舞蹈,很具有观赏性。

原理为多台机器人通过 WIFI 路由器模式在同一个局域网内建立连接后,用户在 PC 上通过 Python 脚本与多台机器人通信,同时向多台机器人下发明文 SDK 指令,从而实现编队控制的功能。在本章节中主要介绍一个用 Python 脚本通过明文 SDK 实现编队控制的简单示例。

4.2. 示例环境

  • 硬件设备:路由器、两台 EP 机器人、 PC
  • Python版本:Python 3.6

4.3. 建立多机连接

将 EP 机器人设置为 WIFI 路由器模式,使用 APP 将参与编队控制的 EP 依次接入同一个路由器中,连接成功后打开 APP 设置中的连接页面,记录 EP 的 IP 地址。具体步骤请参考 WIFI 路由器模式

4.4. 运行示例程序

  1. 将IP地址依次填入参考代码中的 IP_LIST 列表中,并将脚本代码保存为 ep.py。

     1
     2
     3
     4
     5
     6
     7
     8
     9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    89
    #!/usr/bin/env python3
    # coding=utf-8
    
    import sys
    import time
    import threading
    import socket
    
    IP_LIST = ['192.168.1.103', '192.168.1.117']
    EP_DICT = {}
    
    class EP:
            def __init__(self, ip):
                    self._IP = ip
                    self.__socket_ctrl = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                    self.__socket_isRelease = True
                    self.__socket_isConnect = False
                    self.__thread_ctrl_recv = threading.Thread(target=self.__ctrl_recv)
                    self.__seq = 0
                    self.__ack_list = []
                    self.__ack_buf = 'ok'
    
            def __ctrl_recv(self):
                    while self.__socket_isConnect and not self.__socket_isRelease:
                            try:
                                    buf = self.__socket_ctrl.recv(1024).decode('utf-8')
                                    print('%s:%s' % (self._IP, buf))
                                    buf_list = buf.split(' ')
                                    if 'seq' in buf_list:
                                            self.__ack_list.append(int(buf_list[buf_list.index('seq') + 1]))
                                    self.__ack_buf = buf
                            except socket.error as msg:
                                    print('ctrl %s: %s' % (self._IP, msg))
    
            def start(self):
                    try:
                            self.__socket_ctrl.connect((self._IP, 40923))
                            self.__socket_isConnect = True
                            self.__socket_isRelease = False
                            self.__thread_ctrl_recv.start()
                            self.command('command')
                            self.command('robot mode free')
                    except socket.error as msg:
                            print('%s: %s' % (self._IP, msg))
    
            def exit(self):
                    if self.__socket_isConnect and not self.__socket_isRelease:
                            self.command('quit')
                    self.__socket_isRelease = True
                    try:
                            self.__socket_ctrl.shutdown(socket.SHUT_RDWR)
                            self.__socket_ctrl.close()
                            self.__thread_ctrl_recv.join()
                    except socket.error as msg:
                            print('%s: %s' % (self._IP, msg))
    
            def command(self, cmd):
                    self.__seq += 1
                    cmd = cmd + ' seq %d;' % self.__seq
                    print('%s:%s' % (self._IP, cmd))
                    self.__socket_ctrl.send(cmd.encode('utf-8'))
                    timeout = 2
                    while self.__seq not in self.__ack_list and timeout > 0:
                            time.sleep(0.01)
                            timeout -= 0.01
                    if self.__seq in self.__ack_list:
                            self.__ack_list.remove(self.__seq)
                    return self.__ack_buf
    
    if __name__ == "__main__":
            #实例化机器人
            for ip in IP_LIST:
                    print('%s connecting...' % ip)
                    EP_DICT[ip] = EP(ip)
                    EP_DICT[ip].start()
    
            for ip in IP_LIST:
                    EP_DICT[ip].command('gimbal moveto p 0 y 0 vp 90 vy 90 wait_for_complete false')
            time.sleep(3)
    
            while True:
                    for ip in IP_LIST:
                            EP_DICT[ip].command('gimbal moveto p 0 y 45 vp 90 vy 90 wait_for_complete false')
                    time.sleep(3)
                    for ip in IP_LIST:
                            EP_DICT[ip].command('gimbal moveto p 0 y -45 vp 90 vy 90 wait_for_complete false')
                    time.sleep(3)
            for ip in IP_LIST:
                    EP_DICT[ip].exit()
    
  2. 运行脚本

  • Windows系统:完成Python环境后可直接点击 ep.py 启动脚本。
  • Linux系统:在命令终端输入 python ep.py 启动脚本。
  1. 运行效果

编队控制的多台机器人云台步调一致的在 YAW 轴方向往复运动。

../_images/form_control.gif
  1. 运行结果

命令行端口输出多台机器人与主机之间的明文通讯数据。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
192.168.1.103 connecting...
192.168.1.103:command seq 1
192.168.1.103:ok seq 1
192.168.1.103:robot mode free seq 2
192.168.1.103:ok seq 2
192.168.1.117 connecting...
192.168.1.117:command seq 1
192.168.1.117:ok seq 1
192.168.1.117:robot mode free seq 2
192.168.1.117:ok seq 2
192.168.1.103:gimbal moveto p 0 y 0 vp 90 vy 90 wait_for_complete false seq 3
192.168.1.103:ok seq 3
192.168.1.117:gimbal moveto p 0 y 0 vp 90 vy 90 wait_for_complete false seq 3
192.168.1.117:ok seq 3