该程序可以通过键盘对一个或者多个无人机进行速度或者加速度的控制并更改无人机或领导者无人机飞行状态。这个脚本通过发布状态命令和速度命令话题与通讯脚本进行通讯，通讯脚本进而控制无人机飞行。
主要节点：
multirotor_type + '_multirotor_keyboard_control'
#根据传入参数进行设置。
无话题订阅
话题发布：
/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd（std_msgs/String）
/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu(geometry_msgs/Twist)
# 速度模式下发布状态控制与速度控制命令
/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd（std_msgs/String）
/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu(geometry_msgs/Twist)
# 加速度模式下发布状态控制与加速度控制命令
/xtdrone/leader/cmd（std_msgs/String）
/xtdrone/leader/cmd_vel_flu(geometry_msgs/Twist)
/xtdrone/leader/cmd_accel_flu(geometry_msgs/Twist)
# 领导者模式下发布状态控制与速度控制命令
全部代码：
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String
MAX_LINEAR = 20
MAX_ANG_VEL = 3
LINEAR_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.01
cmd_vel_mask = False
ctrl_leader = False
msg2all = """
Control Your XTDrone!
To all drones (press g to control the leader)
---------------------------
1 2 3 4 5 6 7 8 9 0
w r t y i
a s d g j k l
x 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 : mask the keyboard control (for single UAV)
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 0
w r t y i
a s d g j k l
x 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
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 key
def 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']
elif multirotor_num == 1:
formation_configs = ['stop controlling']
cmd= String()
twist = Twist()
rospy.init_node(multirotor_type + '_multirotor_keyboard_control')
if control_type == 'vel':
multi_cmd_vel_flu_pub = [None]*multirotor_num
multi_cmd_pub = [None]*multirotor_num
for i in range(multirotor_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=1)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=3)
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=1)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=1)
else:
multi_cmd_accel_flu_pub = [None]*multirotor_num
multi_cmd_pub = [None]*multirotor_num
for i in range(multirotor_num):
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=1)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=1)
leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=1)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=3)
forward = 0.0
leftward = 0.0
upward = 0.0
angular = 0.0
print_msg()
while(1):
key = getKey()
if key == 'w' :
forward = forward + LINEAR_STEP_SIZE
print_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_SIZE
print_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_SIZE
print_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_SIZE
print_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_SIZE
print_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_SIZE
print_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_SIZE
print_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_SIZE
print_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_leader
print_msg()
elif key in ['k', 's']:
cmd_vel_mask = False
forward = 0.0
leftward = 0.0
upward = 0.0
angular = 0.0
cmd = '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):
cmd = formation_configs[i]
print_msg()
print(cmd)
cmd_vel_mask = True
if (key == '\x03'):
break
if forward > MAX_LINEAR:
forward = MAX_LINEAR
elif forward < -MAX_LINEAR:
forward = -MAX_LINEAR
if leftward > MAX_LINEAR:
leftward = MAX_LINEAR
elif leftward < -MAX_LINEAR:
leftward = -MAX_LINEAR
if upward > MAX_LINEAR:
upward = MAX_LINEAR
elif upward < -MAX_LINEAR:
upward = -MAX_LINEAR
if angular > MAX_ANG_VEL:
angular = MAX_ANG_VEL
elif angular < -MAX_ANG_VEL:
angular = - MAX_ANG_VEL
twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular
for i in range(multirotor_num):
if ctrl_leader:
if control_type == 'vel':
leader_cmd_vel_flu_pub.publish(twist)
else:
leader_cmd_accel_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)
import rospy
from geometry_msgs.msg import Twist # 速度or加速度话题消息类型
import sys, select, os
import tty, termios
from std_msgs.msg import String # 状态控制话题消息类型
MAX_LINEAR = 20 # 最大线速度
MAX_ANG_VEL = 3 # 最大角速度
LINEAR_STEP_SIZE = 0.01 # 每次按键改变的步长
ANG_VEL_STEP_SIZE = 0.01
cmd_vel_mask = False # 速度控制掩码为False时可以控制速度，为True时不能控制速度
ctrl_leader = False # 领导者模式
msg2all = """
Control Your XTDrone!
To all drones (press g to control the leader)
---------------------------
1 2 3 4 5 6 7 8 9 0
w r t y i
a s d g j k l
x 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 : mask the keyboard control (for single UAV)
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 0
w r t y i
a s d g j k l
x 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
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 key
# 获取键盘在终端的按键程序
def print_msg():
if ctrl_leader:
print(msg2leader)
else:
print(msg2all)
# 打印说明字符串
变量声明与定义
# 将传入的参数赋值
multirotor_type = sys.argv[1] # 无人机类型
multirotor_num = int(sys.argv[2]) # 无人机数量
control_type = sys.argv[3] # 控制方式-速度控制or加速度控制
# 多机时的编队形状配置
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']
elif multirotor_num == 1:
formation_configs = ['stop controlling']
cmd= String()
twist = Twist()
启动ROS节点并设置发布话题，如果为多机则for循环设置每个无人机对应的控制话题。
rospy.init_node(multirotor_type + '_multirotor_keyboard_control')
if control_type == 'vel':
multi_cmd_vel_flu_pub = [None]*multirotor_num # 设置数组
multi_cmd_pub = [None]*multirotor_num
for i in range(multirotor_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=1)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=3)
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=1)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=1)
else:
multi_cmd_accel_flu_pub = [None]*multirotor_num
multi_cmd_pub = [None]*multirotor_num
for i in range(multirotor_num):
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=1)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=1)
leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=1)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=3)
程序思路就是获取键盘按键设置forward，leftward，upward，angular四个变量和控制状态变量cmd。由于大多重复，下面举例说明。
if key == 'w' :
forward = forward + LINEAR_STEP_SIZE
print_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时forward变量加一个步长，并且打印信息。
elif key == 't':
cmd = 'ARM'
print_msg()
print('Arming')
这段代码指当键盘输入t时cmd变量设置为ARM并打印信息。
if forward > MAX_LINEAR:
forward = MAX_LINEAR
elif forward < -MAX_LINEAR:
forward = -MAX_LINEAR
限制变量的大小。
twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular
for i in range(multirotor_num):
if ctrl_leader:
if control_type == 'vel':
leader_cmd_vel_flu_pub.publish(twist)
else:
leader_cmd_accel_flu_pub.publish(twist)
leader_cmd_pub.publish(cmd)
else:
if not cmd_vel_mask: # 判断速度控制掩码，掩码为False则发布速度控制话题
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)
最终发布速度与状态话题，实现对无人机的控制。由于编队控制时启用其他速度控制脚本，所以此脚本在编队运行时只发布队形控制命令不发布控制速度话题。
参考资料
程序源码
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）