十、键盘控制无人机 · 下（multirotor_keyboard_control.py解读）

笔记来源于开源项目：基于PX4和ROS的无人机仿真平台

来源于开源项目：GAAS

目录

一、解读启动通信的multirotor_keyboard_control.py脚本

1、库函数以及消息

2、变量及其初始值

3、主函数外的getKey（）和print_msg（）函数

4、main主函数

（1）、初始值的设立

（2）、while循环监听键盘的输入做出反应

二、打开程序，查看节点话题关系图

一、解读启动通信的multirotor_keyboard_control.py脚本

import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import StringMAX_LINEAR = 10
MAX_ANG_VEL = 0.1
LINEAR_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.01cmd_vel_mask = False
ctrl_leader = Falsemsg2all = """
Control Your XTDrone!
To all drones  (press g to control the leader)
---------------------------1   2   3   4   5   6   7   8   9   0w       r    t   y        ia    s    d       g       j    k    lx       v    b   n        ,w/x : increase/decrease forward velocity 
a/d : increase/decrease leftward velocity
i/, : increase/decrease upward velocity
j/l : increase/decrease orientation
r   : return home
t/y : arm/disarm
v/n : takeoff/land
b   : offboard
s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)this will mask the keyboard control
g   : control the leader
CTRL-C to quit
"""msg2leader = """
Control Your XTDrone!
To the leader  (press g to control all drones)
---------------------------1   2   3   4   5   6   7   8   9   0w       r    t   y        ia    s    d       g       j    k    lx       v    b   n        ,w/x : increase/decrease forward velocity
a/d : increase/decrease leftward velocity
i/, : increase/decrease upward velocity
j/l : increase/decrease orientation
r   : return home
t/y : arm/disarm
v/n : takeoff(disenabled now)/land
b   : offboard
s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)this will mask the keyboard control
g   : control all drones
CTRL-C to quit
"""e = """
Communications Failed
"""def getKey():tty.setraw(sys.stdin.fileno())rlist, _, _ = select.select([sys.stdin], [], [], 0.1)if rlist:key = sys.stdin.read(1)else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keydef print_msg():if ctrl_leader:print(msg2leader)else:print(msg2all)      if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)multirotor_type = sys.argv[1]multirotor_num = int(sys.argv[2])control_type = sys.argv[3]if multirotor_num == 18:formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']elif multirotor_num == 9:formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']elif multirotor_num == 6:formation_configs = ['waiting', 'T', 'diamond', 'triangle']cmd= String()twist = Twist()   rospy.init_node('multirotor_keyboard_multi_control')if control_type == 'vel':multi_cmd_vel_flu_pub = [None]*multirotor_nummulti_cmd_pub = [None]*multirotor_numfor i in range(multirotor_num):multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)else:multi_cmd_accel_flu_pub = [None]*multirotor_nummulti_cmd_pub = [None]*multirotor_numfor i in range(multirotor_num):multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)forward  = 0.0leftward  = 0.0upward  = 0.0angular = 0.0print_msg()while(1):key = getKey()if key == 'w' :forward = forward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'x' :forward = forward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'a' :leftward = leftward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'd' :leftward = leftward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'i' :upward = upward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == ',' :upward = upward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'j':angular = angular + ANG_VEL_STEP_SIZEprint_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'l':angular = angular - ANG_VEL_STEP_SIZEprint_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'r':cmd = 'AUTO.RTL'print_msg()print('Returning home')elif key == 't':cmd = 'ARM'print_msg()print('Arming')elif key == 'y':cmd = 'DISARM'print_msg()print('Disarming')elif key == 'v':cmd = 'AUTO.TAKEOFF'cmd = ''print_msg()#print('Takeoff mode is disenabled now')elif key == 'b':cmd = 'OFFBOARD'print_msg()print('Offboard')elif key == 'n':cmd = 'AUTO.LAND'print_msg()print('Landing')elif key == 'g':ctrl_leader = not ctrl_leaderprint_msg()elif key in ['k', 's']:cmd_vel_mask = Falseforward   = 0.0leftward   = 0.0upward   = 0.0angular  = 0.0cmd = 'HOVER'print_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))print('Hover')else:for i in range(10):if key == str(i):print(str(i))cmd = formation_configs[i]print_msg()print(cmd)cmd_vel_mask = Trueif (key == '\x03'):breakif forward > MAX_LINEAR:forward = MAX_LINEARelif forward < -MAX_LINEAR:forward = -MAX_LINEARif leftward > MAX_LINEAR:leftward = MAX_LINEARelif leftward < -MAX_LINEAR:leftward = -MAX_LINEARif upward > MAX_LINEAR:upward = MAX_LINEARelif upward < -MAX_LINEAR:upward = -MAX_LINEARif angular > MAX_ANG_VEL:angular = MAX_ANG_VELelif angular < -MAX_ANG_VEL:angular = - MAX_ANG_VELtwist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upwardtwist.angular.x = 0.0; twist.angular.y = 0.0;  twist.angular.z = angularfor i in range(multirotor_num):if ctrl_leader:if control_type == 'vel':leader_cmd_vel_flu_pub.publish(twist)else:leader_cmd_aceel_flu_pub.publish(twist)leader_cmd_pub.publish(cmd)else:if not cmd_vel_mask:if control_type == 'vel':multi_cmd_vel_flu_pub[i].publish(twist)   else:multi_cmd_accel_flu_pub[i].publish(twist)multi_cmd_pub[i].publish(cmd)cmd = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

1、库函数以及消息

import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String

• from geometry_msgs.msg import Twist：几何数据类型的功能包里面引入Twist话题消息

• from std_msgs.msg import String：标准数据类型功能包里面引入String话题消息

• select模块：专注于I/O多路复用，要理解select.select模块其实主要就是要理解它的参数, 以及其三个返回值

• tty模块：终端控制功能（tty在linux中就是终端的意思）

• termios模块：此模块提供了针对tty I/O 控制的 POSIX 调用的接口。此模块中的所有函数均接受一个文件描述符 fd 作为第一个参数。

• 另外的rospy、sys、os模块就不说了

下面结合具体的函数进行分析

2、变量及其初始值

MAX_LINEAR = 10
MAX_ANG_VEL = 0.1
LINEAR_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.01cmd_vel_mask = False
ctrl_leader = Falsemsg2all = """
Control Your XTDrone!
To all drones  (press g to control the leader)
---------------------------1   2   3   4   5   6   7   8   9   0w       r    t   y        ia    s    d       g       j    k    lx       v    b   n        ,w/x : increase/decrease forward velocity 
a/d : increase/decrease leftward velocity
i/, : increase/decrease upward velocity
j/l : increase/decrease orientation
r   : return home
t/y : arm/disarm
v/n : takeoff/land
b   : offboard
s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)this will mask the keyboard control
g   : control the leader
CTRL-C to quit
"""msg2leader = """
Control Your XTDrone!
To the leader  (press g to control all drones)
---------------------------1   2   3   4   5   6   7   8   9   0w       r    t   y        ia    s    d       g       j    k    lx       v    b   n        ,w/x : increase/decrease forward velocity
a/d : increase/decrease leftward velocity
i/, : increase/decrease upward velocity
j/l : increase/decrease orientation
r   : return home
t/y : arm/disarm
v/n : takeoff(disenabled now)/land
b   : offboard
s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)this will mask the keyboard control
g   : control all drones
CTRL-C to quit
"""e = """
Communications Failed
"""

• MAX_LINEAR=10：表示飞机最大的线速度是10
• MAX_ANG_VEL=0.1：表示飞机最大的角速度是0.1
• LINEAR_STEP_SIZE=0.01：表示飞机改变线速度的时候每次改变0.01
• ANG_STEP_SIZE=0.01：表示飞机改变角速度的时候每次改变0.01
• cmd_vel_mask=False：
• ctrl_leader=False：用来切换控制领导者模式
• msg2all=“......”：To all drones，由于这个控制飞机的程序是基于多机的，因此这个控制模式是所有飞机一起动的控制模式
• msg2leader=“......”：To the leader，这个控制模式是只控制领头的飞机
• e=Communications Failed：出现错误后返回的信息

3、主函数外的getKey（）和print_msg（）函数

getKey()函数主要涉及监听键盘的按键输入（这个整的看不懂写的是啥意思）

大概就是键盘上按了什么按键，它就读出来是是什么按键

def getKey():tty.setraw(sys.stdin.fileno())rlist, _, _ = select.select([sys.stdin], [], [], 0.1)if rlist:key = sys.stdin.read(1)else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return key

• fileno是个文件描述符，文件描述符是一个低级概念，它是一个整数，代表一个打开的文件，也就是说，每打开一个文件都有一个唯一的文件描述符。在在Unix中,按照惯例,三个文件描述符0、1和2分别代表标准输入,标准输出和标准错误。而在Python中输入sys.stdin.fileno()输出为0。
• tty模块定义可函数tty.setraw：tty.setraw(fd, when=termios.TCSAFLUSH)；这个函数将文件描述符 fd 的模式更改为 raw 。如果 when 被省略，则默认为 termios.TCSAFLUSH ，并传递给termios.tcsetattr（）
• sys.stdin：（在Python中的输出是：<_io.TextIOWrapper name='<stdin>' mode='r' encoding='UTF-8'>）
• select.select()方法接收并监控3个通信列表，进程指定内核监听哪些文件描述符(最多监听1024个fd)的哪些事件， 第一个是所有的输入的data,就是指外部发过来的数据，第2个是监控和接收所有要发出去的data(outgoing data),第3个监控错误信息。

fd_r_list, fd_w_list, fd_e_list = select.select(rlist, wlist, xlist, [timeout]) 
参数： 可接受四个参数（前三个必须）
rlist: wait until ready for reading
wlist: wait until ready for writing
xlist: wait for an “exceptional condition”
timeout: 超时时间
返回值：三个列表
select方法用来监视文件描述符(当文件描述符条件不满足时，select会阻塞)，当某个文件描述符状态改变后，会返回三个列表
1、当参数1 序列中的fd满足“可读”条件时，则获取发生变化的fd并添加到fd_r_list中
2、当参数2 序列中含有fd时，则将该序列中所有的fd添加到 fd_w_list中
3、当参数3 序列中的fd发生错误时，则将该发生错误的fd添加到 fd_e_list中
4、当超时时间为空，则select会一直阻塞，直到监听的句柄发生变化当超时时间 ＝ n(正整数)时，那么如果监听的句柄均无任何变化，则select会阻塞n秒，之后返回三个空列表，如果监听的句柄有变化，则直接执行。

• 如果序列中的fd满足“可读”条件时，则获取发生变化的fd并添加到rlist中，然后读取出key的值

• termios.tcsetattr(fd, when, attributes)

  根据 attributes 列表设置文件描述符 fd 的 tty 属性，该列表即 tcgetattr() 所返回的对象。 when 参数确定何时改变属性: TCSANOW 表示立即改变，TCSADRAIN 表示在传输所有队列输出后再改变，或 TCSAFLUSH 表示在传输所有队列输出并丢失所有队列输入后再改变。

关于函数print_msg（）还是很好理解的，只要控制了ctrl_leader的输出是True还是False，就可以控制输出是全局控制模式还是领导控制模式：

def print_msg():if ctrl_leader:print(msg2leader)else:print(msg2all)    

4、main主函数

if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)multirotor_type = sys.argv[1]multirotor_num = int(sys.argv[2])control_type = sys.argv[3]if multirotor_num == 18:formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']elif multirotor_num == 9:formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']elif multirotor_num == 6:formation_configs = ['waiting', 'T', 'diamond', 'triangle']cmd= String()twist = Twist()   rospy.init_node('multirotor_keyboard_multi_control')if control_type == 'vel':multi_cmd_vel_flu_pub = [None]*multirotor_nummulti_cmd_pub = [None]*multirotor_numfor i in range(multirotor_num):multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)else:multi_cmd_accel_flu_pub = [None]*multirotor_nummulti_cmd_pub = [None]*multirotor_numfor i in range(multirotor_num):multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)forward  = 0.0leftward  = 0.0upward  = 0.0angular = 0.0print_msg()while(1):key = getKey()if key == 'w' :forward = forward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'x' :forward = forward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'a' :leftward = leftward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'd' :leftward = leftward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'i' :upward = upward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == ',' :upward = upward - LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'j':angular = angular + ANG_VEL_STEP_SIZEprint_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'l':angular = angular - ANG_VEL_STEP_SIZEprint_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))elif key == 'r':cmd = 'AUTO.RTL'print_msg()print('Returning home')elif key == 't':cmd = 'ARM'print_msg()print('Arming')elif key == 'y':cmd = 'DISARM'print_msg()print('Disarming')elif key == 'v':cmd = 'AUTO.TAKEOFF'cmd = ''print_msg()#print('Takeoff mode is disenabled now')elif key == 'b':cmd = 'OFFBOARD'print_msg()print('Offboard')elif key == 'n':cmd = 'AUTO.LAND'print_msg()print('Landing')elif key == 'g':ctrl_leader = not ctrl_leaderprint_msg()elif key in ['k', 's']:cmd_vel_mask = Falseforward   = 0.0leftward   = 0.0upward   = 0.0angular  = 0.0cmd = 'HOVER'print_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))print('Hover')else:for i in range(10):if key == str(i):print(str(i))cmd = formation_configs[i]print_msg()print(cmd)cmd_vel_mask = Trueif (key == '\x03'):breakif forward > MAX_LINEAR:forward = MAX_LINEARelif forward < -MAX_LINEAR:forward = -MAX_LINEARif leftward > MAX_LINEAR:leftward = MAX_LINEARelif leftward < -MAX_LINEAR:leftward = -MAX_LINEARif upward > MAX_LINEAR:upward = MAX_LINEARelif upward < -MAX_LINEAR:upward = -MAX_LINEARif angular > MAX_ANG_VEL:angular = MAX_ANG_VELelif angular < -MAX_ANG_VEL:angular = - MAX_ANG_VELtwist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upwardtwist.angular.x = 0.0; twist.angular.y = 0.0;  twist.angular.z = angularfor i in range(multirotor_num):if ctrl_leader:if control_type == 'vel':leader_cmd_vel_flu_pub.publish(twist)else:leader_cmd_aceel_flu_pub.publish(twist)leader_cmd_pub.publish(cmd)else:if not cmd_vel_mask:if control_type == 'vel':multi_cmd_vel_flu_pub[i].publish(twist)   else:multi_cmd_accel_flu_pub[i].publish(twist)multi_cmd_pub[i].publish(cmd)cmd = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

（1）、初始值的设立

• settings = termios.tcgetattr(sys.stdin)：这个和键盘的输入有关

• multirotor_type = sys.argv[1]：返回输入执行Python文件时候后面跟着的第一个参数：飞机的机型

  multirotor_num = int(sys.argv[2])：返回第二个参数：飞机的数量

  control_type = sys.argv[3]：返回第三个参数：控制的类型（是速度vel控制，还是加速度控制）

• 现在针对多机情况：如果飞机数量是18,9,6，这里就有对应的formation_config（飞行结构配置，飞行队形把应该）
• 设置消息：cmd、twist

• rospy.init_node('multirotor_keyboard_multi_control')：初始化节点，节点的名字叫做multirotor_keyboard_multi_control（无人机键盘的多样控制）

• if-else control_type：读取上面的参数之后可以确定control_type的值，然后通过这个值（vel或者空）来判断是速度控制还是加速度控制。我们选取速度（vel）控制来分析：for i in range(multirotor_num)表示对每一架飞机都进行创建发布控制指令消息String以及速度控制消息Twist的Publisher，然后对于领导飞机有专门创建发布领导飞机的控制指令消息String以及速度控制消息Twist的Publisher

• 初始化 forward、leftward、upward 、angular四个方向的速度都为0.0这个基础值

• print_msg()在终端输出对应的键盘控制模式图

（2）、while循环监听键盘的输入做出反应

key  = getKey()表示的key得到为键盘按键的值

下面的程序几乎一个样子，选择一个来具体分析一下：

if key == 'w' :forward = forward + LINEAR_STEP_SIZEprint_msg()if control_type == 'vel':print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))else:print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))

当有按键按下为”w“的时候，forword的值就是原来的值再加上按一次就加上一次速度增加的步长

然后在输出一次现在的控制模式

如果现在的控制模式是速度模式，那么就在终端输出此刻的f，l，u，a的值

elif key == 'r':cmd = 'AUTO.RTL'print_msg()print('Returning home')

当有按键按下为”r“的时候就把cmd的模式调整为'AUTO.RTL'，还记得这个cmd吗，前面cmd=String（），然后是通过这个消息发布出去的。那你还记得multirotor_communication.py这个上一篇教程解读里面不是有几个订阅消息的subscriber嘛，比如，就是这里的cmd发布出去指令的string（）消息，然后上一篇的通讯连接中的cmd订阅到然后做出分析的。

这两个可以提出来给大家看一下：

        elif key == 'g':ctrl_leader = not ctrl_leaderprint_msg()elif key in ['k', 's']:cmd_vel_mask = Falseforward   = 0.0leftward   = 0.0upward   = 0.0angular  = 0.0cmd = 'HOVER'print_msg()print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))print('Hover')

这一段代码是配置多机的飞机队伍的时候用到的，看看就行了：

感觉源代码的格式有点问题，我就调整了一下空格：

        else:for i in range(10):if key == str(i):print(str(i))cmd = formation_configs[i]print_msg()print(cmd)cmd_vel_mask = Trueif (key == '\x03'):break

下面是一大段对最大值的比较，就是不能超过最大值

同时，这里也定义了数据消息Twist的值：

twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upwardtwist.angular.x = 0.0; twist.angular.y = 0.0;  twist.angular.z = angular

然后每一只飞机都发布出去新的位置消息（之前那个是在循环外面的，是创建好了要发布所有的节点的消息以及初始位置的消息的Publisher）：

        for i in range(multirotor_num):if ctrl_leader:if control_type == 'vel':leader_cmd_vel_flu_pub.publish(twist)else:leader_cmd_aceel_flu_pub.publish(twist)leader_cmd_pub.publish(cmd)else:if not cmd_vel_mask:if control_type == 'vel':multi_cmd_vel_flu_pub[i].publish(twist)   else:multi_cmd_accel_flu_pub[i].publish(twist)multi_cmd_pub[i].publish(cmd)

最后的这个一条也和键盘的输入有关吧，反正不是很明白：

 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

二、打开程序，查看节点话题关系图