Управление дроном с клавиатуры (WASD)

import select                                  # select нужен для чтения клавиш без блокировки программы
import sys                                     # sys нужен для доступа к вводу с клавиатуры через stdin
import termios                                 # termios нужен для настройки терминала
import time                                    # time нужен для задержек и таймеров
import tty                                     # tty нужен для чтения клавиш без нажатия Enter

from pioneer_sdk2 import Pioneer, Camera, ImageViewer # импортируем управление дроном, камерой и видеопотоком


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),          # поворот вправо
}


def read_key():                                # функция читает последнюю нажатую клавишу
    key = None                                 # None означает, что новых клавиш нет

    while select.select([sys.stdin], [], [], 0)[0]: # читаем все символы, которые уже пришли в терминал
        key = sys.stdin.read(1)                # берем один символ
        if key != "\x1b":                      # Esc оставляем как есть
            key = key.lower()                  # остальные клавиши приводим к нижнему регистру

    return key                                 # возвращаем последнюю клавишу из очереди


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

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

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

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

if not sys.stdin.isatty():                     # проверяем, что программа запущена в интерактивном терминале
    raise RuntimeError("Запустите пример в интерактивном SSH-терминале")

drone = Pioneer()                              # создаем объект управления дроном
camera = Camera()                              # создаем объект камеры
viewer = ImageViewer()                         # создаем публикацию видеопотока
terminal_settings = termios.tcgetattr(sys.stdin) # сохраняем настройки терминала, чтобы восстановить их при выходе

print("Видеопоток: rtsp://10.42.0.1:8889/video/")

last_motion_key = None                         # последняя клавиша движения
last_motion_time = 0.0                         # время последнего получения клавиши движения
last_send_time = 0.0                           # время последней отправки скорости
last_sent_speed = None                         # последняя отправленная скорость

try:
    tty.setcbreak(sys.stdin.fileno())          # включаем чтение клавиш без Enter

    while True:
        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)

        now = time.monotonic()                 # текущее время для таймеров
        key = read_key()                       # читаем клавишу, если она была нажата
        state = drone.get_fly_state().name     # состояние дрона: ON_LAND, ARMED или IN_SKY

        if key in SPEED_BY_KEY:                # если нажата клавиша движения
            last_motion_key = key              # запоминаем ее
            last_motion_time = now             # обновляем время последнего нажатия

        elif key == "x":                       # X - остановка движения
            last_motion_key = None             # сбрасываем активную клавишу, дальше отправятся нулевые скорости

        elif key == "1" and state == "ON_LAND": # 1 - включить двигатели
            drone.arm()

        elif key == "2" and state == "ARMED":  # 2 - выключить двигатели до взлета
            drone.disarm()

        elif key == "3" and state != "IN_SKY": # 3 - взлет
            if state == "ON_LAND":             # если двигатели еще не включены
                drone.arm()                    # включаем их перед взлетом
            if drone.get_fly_state().name == "ARMED": # проверяем, что двигатели включились
                drone.takeoff()                # взлетаем
                last_sent_speed = None         # после взлета сразу отправим первую скорость

        elif key == "4" and state == "IN_SKY": # 4 - посадка
            drone.set_manual_speed_body_fixed(*ZERO_SPEED, COMMAND_INTERVAL) # сначала остановим движение
            drone.land()                       # затем сядем
            last_motion_key = None
            last_sent_speed = None

        elif key == "\x1b":                    # Esc - выход из программы
            break

        # В 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

        time.sleep(MAIN_LOOP_DELAY)

finally:
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, terminal_settings) # возвращаем терминал в обычный режим

    viewer.close()                             # закрываем публикацию видеопотока
    camera.stop()                              # останавливаем камеру

    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()                   # закрываем соединение с дроном