AirSim 使用Pygame鼠标键盘控制无人机

使用鼠标和键盘控制无人机的代码如下

python 复制代码

import pygame
import sys
import airsim
import time
#pygame的设置
pygame.init()
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption('Drone')
screen.fill((0, 0, 0))

#airsim的设置
vehicle_name = 'Drone'
AirSim_client = airsim.MultirotorClient()#建立通信
AirSim_client.confirmConnection()#确认连接
AirSim_client.enableApiControl(True,vehicle_name=vehicle_name)#获取API控制
AirSim_client.armDisarm(True,vehicle_name=vehicle_name)#解锁桨叶
AirSim_client.takeoffAsync(vehicle_name=vehicle_name).join()#起飞

#基础的飞行速度
vehicle_velocity=2.0
#设置临时加速度
speedup_ratio=10.0
#设置临时加速度
speedup_flag=False
#基础偏航角速率
vehicle_yaw_rate=5.0

while True:
    yaw_rate=0.0#初始都为0
    velocity_x=0.0
    velocity_y=0.0
    velocity_z=0.0
    time.sleep(0.02)

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            sys.exit()
    scan_wrapper=pygame.key.get_pressed()
    if scan_wrapper[pygame.K_SPACE]:#按下空格，加速10倍
        scale_ratio=speedup_ratio
    else:
        scale_ratio=speedup_ratio/speedup_ratio
    #使用A或者D来设置偏航速度
    if scan_wrapper[pygame.K_a] or scan_wrapper[pygame.K_d]:
        yaw_rate=(scan_wrapper[pygame.K_d]-scan_wrapper[pygame.K_a])*scale_ratio*vehicle_yaw_rate#按下d，d为1，a为0，正向转动，a键反向转动
    if scan_wrapper[pygame.K_UP] or scan_wrapper[pygame.K_DOWN]:
        velocity_x=(scan_wrapper[pygame.K_UP]-scan_wrapper[pygame.K_DOWN])*scale_ratio
    if scan_wrapper[pygame.K_LEFT] or scan_wrapper[pygame.K_RIGHT]:
        velocity_y=-(scan_wrapper[pygame.K_LEFT]-scan_wrapper[pygame.K_RIGHT])*scale_ratio
    if scan_wrapper[pygame.K_w] or scan_wrapper[pygame.K_s]:
        velocity_z=-(scan_wrapper[pygame.K_w]-scan_wrapper[pygame.K_s])*scale_ratio#添加-号是为了和世界坐标系同向

    AirSim_client.moveByVelocityAsync(vx=velocity_x,vy=velocity_y,vz=velocity_z,duration=0.02,#使用速度控制无人机
                                      yaw_mode=airsim.YawMode(True,yaw_or_rate=yaw_rate),
                                      vehicle_name=vehicle_name).join()
    if scan_wrapper[pygame.K_ESCAPE]:
        pygame.quit()
        sys.exit()

首先要下载pygame，可以在conda的环境下使用命令下载。

python 复制代码

pip install -i https://pypi.tuna.tsinghua.edu.cn/simple pygame

1. Pygame 初始化与设置

python 复制代码

pygame.init()
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption('Drone')
screen.fill((0, 0, 0))

使用pygame.init()初始化，

pygame.display.set_mode()创建了一个窗口对象，窗口的宽度为 640 像素，高度为 480 像素。

调用接口pygame.display.set_caption('Drone')：设置窗口标题为 "Drone"，

使用screen的接口函数，screen.fill((0, 0, 0))，rgb的值为0，将窗口背景填充为黑色。

2.AirSim 设置

python 复制代码

vehicle_name = 'Drone'
AirSim_client = airsim.MultirotorClient()#建立通信
AirSim_client.confirmConnection()#确认连接
AirSim_client.enableApiControl(True,vehicle_name=vehicle_name)#获取API控制
AirSim_client.armDisarm(True,vehicle_name=vehicle_name)#解锁桨叶
AirSim_client.takeoffAsync(vehicle_name=vehicle_name).join()#起飞

vehicle_name = 'Drone'这里无人机的名称是在setting.json里面的名称，在C:\Users\FEG\Documents\AirSim\settings.json里

之后建立对象AirSim_client，调用接口确认连接，获得api的控制权，解锁桨叶最后起飞。具体可以看我前一篇博客的解析。

3. 变量初始化

python 复制代码

#基础的飞行速度
vehicle_velocity=2.0
#设置临时加速度
speedup_ratio=10.0
#设置临时加速度
speedup_flag=False
#基础偏航角速率
vehicle_yaw_rate=5.0

这里对我们后续使用的变量先进行初始化。

4.进入主循环

python 复制代码

while True:
    yaw_rate=0.0#初始都为0
    velocity_x=0.0
    velocity_y=0.0
    velocity_z=0.0
    time.sleep(0.02)

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            sys.exit()
    scan_wrapper=pygame.key.get_pressed()
    if scan_wrapper[pygame.K_SPACE]:#按下空格，加速10倍
        scale_ratio=speedup_ratio
    else:
        scale_ratio=speedup_ratio/speedup_ratio
    #使用A或者D来设置偏航速度
    if scan_wrapper[pygame.K_a] or scan_wrapper[pygame.K_d]:
        yaw_rate=(scan_wrapper[pygame.K_d]-scan_wrapper[pygame.K_a])*scale_ratio*vehicle_yaw_rate#按下d，d为1，a为0，正向转动，a键反向转动
    if scan_wrapper[pygame.K_UP] or scan_wrapper[pygame.K_DOWN]:
        velocity_x=(scan_wrapper[pygame.K_UP]-scan_wrapper[pygame.K_DOWN])*scale_ratio
    if scan_wrapper[pygame.K_LEFT] or scan_wrapper[pygame.K_RIGHT]:
        velocity_y=-(scan_wrapper[pygame.K_LEFT]-scan_wrapper[pygame.K_RIGHT])*scale_ratio
    if scan_wrapper[pygame.K_w] or scan_wrapper[pygame.K_s]:
        velocity_z=-(scan_wrapper[pygame.K_w]-scan_wrapper[pygame.K_s])*scale_ratio#添加-号是为了和世界坐标系同向

    AirSim_client.moveByVelocityAsync(vx=velocity_x,vy=velocity_y,vz=velocity_z,duration=0.02,#使用速度控制无人机
                                      yaw_mode=airsim.YawMode(True,yaw_or_rate=yaw_rate),
                                      vehicle_name=vehicle_name).join()
    if scan_wrapper[pygame.K_ESCAPE]:
        pygame.quit()
        sys.exit()

该循环是一个无限循环。

python 复制代码

yaw_rate = 0.0
velocity_x = 0.0
velocity_y = 0.0
velocity_z = 0.0
time.sleep(0.02)  # 每次循环延迟 20 毫秒

1.速度初始化

每次循环开始时，将速度和偏航速率初始化为 0，并延迟20ms。

之后进行事件检测

2.事件检测

python 复制代码

for event in pygame.event.get():

pygame.event.get()可以获取用户输入事件，如键盘按下的键，鼠标的点击等。

if event.type == pygame.QUIT，检测到用户点击了窗口上的 "X" 关闭按钮，退出程序。

之后进行控制的读取设置。

3. 键盘控制逻辑

1.加速键

python 复制代码

    if scan_wrapper[pygame.K_SPACE]:#按下空格，加速10倍
        scale_ratio=speedup_ratio

Pygame 为所有键盘按键定义了对应的常量，统一的命名规则如下：

pygame.K_ 作为前缀。

后面跟着对应按键的名称（通常是大写字母或字符的表示）

Pygame 为所有键盘按键定义了对应的常量，统一的命名规则如下：

pygame.K_ 作为前缀。

后面跟着对应按键的名称（通常是大写字母或字符的表示）

这里pygame.K_SPACE就表示SPACE空格键，后面pygame.K_a就表示字母a按键，pygame.K_UP, pygame.K_DOWN, pygame.K_LEFT, pygame.K_RIGHT就表示箭头上下左右键

python 复制代码

scan_wrapper=pygame.key.get_pressed()

这里scan_wrapper 是 pygame.key.get_pressed() 函数的返回值，它是一个布尔值列表，如果某个按键被按下，对应索引的值为 True，否则为 False。

scan_wrapper[pygame.K_SPACE]中，pygame.K_SPACE是一个常量值表示空格键，将其作为索引判断空格键是true还是false，是否被按下。

python 复制代码

 if scan_wrapper[pygame.K_SPACE]:#按下空格，加速10倍
        scale_ratio=speedup_ratio
    else:
        scale_ratio=speedup_ratio/speedup_ratio

当空格键按下时，速度倍增为 speedup_ratio（10 倍加速），否则倍增为1倍，即为原来速度。

2.偏航键

python 复制代码

    if scan_wrapper[pygame.K_a] or scan_wrapper[pygame.K_d]:
        yaw_rate=(scan_wrapper[pygame.K_d]-scan_wrapper[pygame.K_a])*scale_ratio*vehicle_yaw_rate

当列表中检测到a或者d键按下时，要执行偏航操作。

python 复制代码

yaw_rate = (scan_wrapper[pygame.K_d] - scan_wrapper[pygame.K_a]) * scale_ratio * vehicle_yaw_rate

如果按下d，则scan_wrapper[pygame.K_d]为true，即1，scan_wrapper[pygame.K_a]为false，即0，则(scan_wrapper[pygame.K_d] - scan_wrapper[pygame.K_a])为1，所以，(scan_wrapper[pygame.K_d] - scan_wrapper[pygame.K_a])是来设置偏航方向的，d为正向偏航，a为负向偏航。偏航速度=正负号*加速度*偏航速度。

3.水平和垂直移动

python 复制代码

    if scan_wrapper[pygame.K_UP] or scan_wrapper[pygame.K_DOWN]:
        velocity_x=(scan_wrapper[pygame.K_UP]-scan_wrapper[pygame.K_DOWN])*scale_ratio
    if scan_wrapper[pygame.K_LEFT] or scan_wrapper[pygame.K_RIGHT]:
        velocity_y=-(scan_wrapper[pygame.K_LEFT]-scan_wrapper[pygame.K_RIGHT])*scale_ratio
    if scan_wrapper[pygame.K_w] or scan_wrapper[pygame.K_s]:
        velocity_z=-(scan_wrapper[pygame.K_w]-scan_wrapper[pygame.K_s])*scale_ratio

箭头上下键用于控制无人机沿x方向的运动，箭头左右键用于控制无人机沿y方向的运行，w和s键用于控制无人机沿z方向的垂直运动。-(scan_wrapper[pygame.K_w]-scan_wrapper[pygame.K_s])这里添加了-号，因为在机体坐标系中，z轴是垂直向下的和世界坐标系相反。

4.调用 AirSim API 进行无人机控制

python 复制代码

AirSim_client.moveByVelocityAsync(vx=velocity_x,vy=velocity_y,vz=velocity_z,duration=0.02,#使用速度控制无人机
                                      yaw_mode=airsim.YawMode(True,yaw_or_rate=yaw_rate),
                                      vehicle_name=vehicle_name).join()

使用moveByVelocityAsync()，传入参数vx，vy，vz和持续事件duration。

python 复制代码

yaw_mode=airsim.YawMode(True, yaw_or_rate=yaw_rate)

YawMode 是 AirSim 中用于控制飞行器偏航的一个类，第一个参数True表示启动偏航控制，即，如果设置为 True，飞行器的偏航（旋转）会受到控制（由 yaw_or_rate 控制的偏航速率来调整）。如果设置为 False，飞行器将根据当前的飞行方向自动调整偏航，不会再接受用户的偏航控制。

yaw_or_rate=yaw_rate第二个参数是偏航控制方式，这里使用我们按键获得的偏航速度进行偏航操作，按下 a 键时，yaw_rate 为负值（逆时针旋转）；按下 d 键时，yaw_rate 为正值（顺时针旋转）。

vehicle_name=vehicle_name把控制的vehicle设为我们settings文件里面的vehicle，由于是异步函数，要使用join()等待操作完成后再实现下一个操作。

5. 退出程序

python 复制代码

    if scan_wrapper[pygame.K_ESCAPE]:
        pygame.quit()
        sys.exit()

检测到 ESC 键被按下时退出pygame，退出python程序。