Разбор: WASD_flight.py

Источник: репозиторий pioneer-team/pioneer-sdk2-example (GitFlic). Справочник по классам API — на странице «Pioneer SDK 2».

Интерактивное управление с клавиатуры

Один файл — интерактивный «пульт»: W/S/A/D — движение, Space/Z — вверх/вниз, Q/E — повороты, 1–4 — arm/disarm/takeoff/land, Esc — посадка и выход. Одновременно публикуется RTSP-поток с камеры. Требуется интерактивный SSH-терминал или консоль на борту:

print(
    """
Управление Pioneer Mini 2 через SSH

1 -- arm        2 -- disarm
3 -- takeoff    4 -- land

↶q  w↑  e↷     Space -- вверх
←a      d→      z     -- вниз
    s↓

x -- стоп, Esc -- посадка и выход
"""
)

Поток с видео с камеры: rtsp://10.42.0.1:8889/video/.

Таблица скоростей

Клавиши движения отображаются на команды set_manual_speed_body_fixed() через словарь SPEED_BY_KEY; сами скорости фиксированы константами в начале файла:

LINEAR_SPEED = 0.2                             # скорость движения вперед/назад/влево/вправо в м/с
VERTICAL_SPEED = 0.2                           # скорость движения вверх/вниз в м/с
YAW_SPEED = 0.5                                # скорость поворота в рад/с

COMMAND_INTERVAL = 0.3                         # время действия одной команды скорости в секундах
SEND_PERIOD = 0.2                              # как часто повторять команду скорости, включая нулевую
KEY_TIMEOUT = 0.6                              # через сколько секунд без повтора считать клавишу отпущенной
MAIN_LOOP_DELAY = 0.02                         # небольшая задержка главного цикла

ZERO_SPEED = (0.0, 0.0, 0.0, 0.0)              # нулевая команда: vx, vy, vz, yaw_rate

SPEED_BY_KEY = {                               # таблица соответствия клавиш и скоростей
    "w": (0.0, LINEAR_SPEED, 0.0, 0.0),        # вперед относительно корпуса дрона
    "s": (0.0, -LINEAR_SPEED, 0.0, 0.0),       # назад относительно корпуса дрона
    "a": (-LINEAR_SPEED, 0.0, 0.0, 0.0),       # влево относительно корпуса дрона
    "d": (LINEAR_SPEED, 0.0, 0.0, 0.0),        # вправо относительно корпуса дрона
    " ": (0.0, 0.0, VERTICAL_SPEED, 0.0),      # вверх
    "z": (0.0, 0.0, -VERTICAL_SPEED, 0.0),     # вниз
    "q": (0.0, 0.0, 0.0, YAW_SPEED),           # поворот влево
    "e": (0.0, 0.0, 0.0, -YAW_SPEED),          # поворот вправо
}

Обратите внимание: q — поворот влево (положительный yaw_rate), e — вправо (отрицательный); скорости в рад/с, а линейные — в м/с.

Главный цикл: таймеры вместо событий

В SSH-терминале нет события «клавишу отпустили». Скрипт решает это таймером: если автоповтор клавиши давно не приходил, считается, что стики вернулись в нейтраль:

        # В SSH-терминале нет события "клавишу отпустили".
        # Поэтому если автоповтор клавиши давно не приходил, считаем, что клавишу отпустили.
        if last_motion_key is not None and now - last_motion_time > KEY_TIMEOUT:
            last_motion_key = None

        speed = SPEED_BY_KEY.get(last_motion_key, ZERO_SPEED) # движение по клавише или нулевая скорость
        send_due = time.monotonic() - last_send_time >= SEND_PERIOD # пора повторить команду скорости
        speed_changed = speed != last_sent_speed # новую скорость отправляем сразу

        if drone.get_fly_state().name == "IN_SKY" and (speed_changed or send_due):
            drone.set_manual_speed_body_fixed(*speed, COMMAND_INTERVAL)
            last_send_time = time.monotonic()
            last_sent_speed = speed

Константа	Значение	Смысл
`KEY_TIMEOUT`	0,6 с	Пауза после последнего автоповтора: клавиша считается отпущенной
`SEND_PERIOD`	0,2 с	Как часто повторять команду, включая нулевую
`COMMAND_INTERVAL`	0,3 с	Время действия одной команды (команды должны перекрываться)

Задача «отправить команду вовремя» решается двумя флагами: speed_changed — новую скорость отправляем немедленно; send_due — неизменённую команду дублируем по таймеру, чтобы дрон не «отвалился» в ручной режим.

Видео в том же цикле

Кадры получаются в главном цикле, без отдельного потока; ошибка видеотракта управление не прерывает:

        try:                                   # получаем кадр камеры в основном цикле, без отдельного потока
            frame = camera.get_cv_frame(timeout=0.05)
            if frame is not None:
                viewer.imshow("video", frame, fps=30)
        except TimeoutError:                   # если кадр не успел прийти, продолжаем управление
            pass
        except Exception as error:             # ошибка камеры не должна завершать программу управления
            print("Ошибка видеопотока:", error)
            time.sleep(0.2)

Штатные команды и завершение

Клавиши-действия превращаются в команды SDK: 1 — arm(), 2 — disarm(), 3 — takeoff() (с предварительным arm() при необходимости), 4 — нулевая скорость и land(). Проверка sys.stdin.isatty() в начале программно запрещает запуск без терминала. Безопасный выход гарантирован finally:

    state = drone.get_fly_state().name         # проверяем состояние перед выходом
    if state == "IN_SKY":                      # если программа завершается в полете
        drone.set_manual_speed_body_fixed(*ZERO_SPEED, COMMAND_INTERVAL)
        drone.land()                           # сажаем дрон
    elif state == "ARMED":                     # если двигатели включены, но взлета не было
        drone.disarm()                         # выключаем двигатели

    drone.close_connection()                   # закрываем соединение с дроном

Если программа завершается в полёте (IN_SKY), дрон получает нулевую скорость и садится; если двигатели включены, но взлёта не было (ARMED) — они выключаются командой disarm().