Разбор: mission_examples

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

Что внутри и запуск

Четыре законченных полётных сценария класса Pioneer:

go_to_local_point.py — полёт по нескольким локальным точкам с ожиданием события POINT_REACHED.
circle_flight.py — движение по точкам окружности с видеопотоком video.
mission_with_frames.py — полёт по списку точек с видеопотоком pioneer_camera.
set_manual_speed.py — ручное задание скорости через set_manual_speed().

Скрипты запускаются там, где установлен pioneer_sdk2 (см. способы запуска):

python3 go_to_local_point.py
python3 circle_flight.py
python3 mission_with_frames.py
python3 set_manual_speed.py

go_to_local_point.py — полёт по точкам с событием

Дрон взлетает и последовательно летит в три точки: набирает высоту 1 м, смещается на 1 м по Y, затем на 1 м по X. Главная идея примера — не «спать лишнюю минуту», а дождаться прилёта по событию автопилота POINT_REACHED:

from pioneer_sdk2 import Pioneer              # импортируем класс Pioneer из библиотеки pioneer_sdk2
import pioneer_sdk2                           # импортируем библиотеку pioneer_sdk2 для доступа к событиям
import threading                              # библиотека threading нужна для работы с событием Event
import time                                   # библиотека time содержит функции для работы со временем


drone = Pioneer()                             # создаем экземпляр класса Pioneer, устанавливаем соединение
point_event = threading.Event()               # создаем событие, которое сработает при достижении точки

def point_reached(event):                     # функция вызывается, когда дрон достигает заданной точки
    point_event.set()                         # сообщаем программе, что точка достигнута

def wait_for_point():                         # функция ожидания прилета дрона в точку
    point_event.wait()                        # ждем, пока дрон достигнет заданной точки
    point_event.clear()                       # очищаем событие для следующего ожидания

drone.subscribe(point_reached, pioneer_sdk2.Event.POINT_REACHED) # подписываемся на событие достижения точки

try:                                          # основной код находится внутри блока try
    drone.arm()                               # включаем двигатели
    drone.takeoff()                           # взлетаем

    time.sleep(3)                             # ставим паузу на 3 секунды после взлета

    drone.go_to_local_point(x=0, y=0, z=1, yaw=0, time=3) # летим в точку с координатами x=0, y=0, z=1
                                                          # x, y, z - координаты точки в метрах
                                                          # yaw - поворот по курсу в радианах
                                                          # time - время, за которое требуется достигнуть точку
    wait_for_point()                                      # ждем, пока дрон долетит до заданной точки

    drone.go_to_local_point(x=0, y=1, z=1, yaw=0, time=3) # летим в первую точку с координатами x=0, y=1, z=1
    wait_for_point()                                      # ждем, пока дрон долетит до заданной точки

    drone.go_to_local_point(x=1, y=1, z=1, yaw=0, time=3) # летим во вторую точку с координатами x=1, y=1, z=1
    wait_for_point()                                      # ждем, пока дрон долетит до заданной точки

    drone.land()                              # сажаем дрон, двигатели выключатся автоматически

except KeyboardInterrupt:                     # если пользователь остановил программу сочетанием Ctrl+C
    print("Остановка программы, производится посадка") # выводим сообщение об остановке программы
    drone.land()                                       # сажаем дрон

except Exception as error:                    # если произошла любая другая ошибка
    print("Ошибка:", error)                   # выводим текст ошибки
    drone.land()                              # сажаем дрон при ошибке

finally:                                      # блок finally выполнится в любом случае
    drone.close_connection()                  # закрываем соединение с дроном

Как это работает:

Pioneer() устанавливает соединение — после этого можно отправлять команды.
go_to_local_point(x, y, z, yaw, time) — не блокирующий метод: после вызова программу нужно «остановить» ожиданием, иначе следующая команда перебьёт текущую точку.
Ожидание построено на событии: drone.subscribe(point_reached, Event.POINT_REACHED) регистрирует колбэк, который при прилёте делает set() у threading.Event. Служебная функция wait_for_point() блокируется на point_event.wait() и очищает флаг clear() для следующего ожидания.
Точки задаются в метрах в локальной системе координат; time — время, за которое нужно достичь точку (наши три перелёта по 3 с).
arm() и takeoff() обязательно предшествуют полётам: без состояний ARMED/IN_SKY команды полёта игнорируются автопилотом.
Оба except гарантируют land() при Ctrl+C и любой ошибке; finally всегда закрывает соединение.

Метод подробно разобран в справочнике: «Полёт в координаты (локальные)».

circle_flight.py — движение по окружности

Дрон облетает окружность из 8 точек радиусом 0,7 м на высоте 1 м и по пути транслирует видео в поток video. Координаты точки окружности считаются классической тригонометрией:

circle_radius = 0.7                                   # радиус окружности в метрах
circle_points = 8                                     # количество точек на окружности
circle_angle = 0                                      # стартовый угол на окружности в градусах
z = 1.0                                               # высота полета в метрах


def get_point_on_circle(angle, radius):               # функция расчета координат точки на окружности
    radians = math.radians(angle)                     # переводим угол из градусов в радианы
    x = radius * math.cos(radians)                    # вычисляем координату точки по оси X
    y = radius * math.sin(radians)                    # вычисляем координату точки по оси Y
    return x, y                                       # возвращаем координаты X и Y

Начальные настройки: радиус, число точек, стартовый угол и высота — обычные переменные в начале файла. Полёт сводится к циклу «рассчитать точку → лететь → дождаться»:

    while circle_angle < 360:                             # выполняем полет, пока не пройдем полный круг
        frame = camera.get_cv_frame(timeout=1.0)          # получаем один кадр с камеры
                                                          # timeout=1.0 - время ожидания кадра в секундах

        if frame is not None:                             # проверяем, что кадр успешно получен
            video_drone.imshow("video", frame, fps=30)    # запускаем трансляцию видео
                                                          # video - название трансляции
                                                          # frame - изображение с камеры
                                                          # fps=30 - количество кадров в секунду

        x, y = get_point_on_circle(circle_angle, circle_radius) # рассчитываем следующую точку окружности

        drone.go_to_local_point(x=x, y=y, z=z, yaw=0, time=3)   # летим в рассчитанную точку окружности
        wait_for_point()                                        # ждем, пока дрон долетит до точки

        circle_angle += 360 / circle_points             # увеличиваем угол для перехода к следующей точке

    drone.land()                                        # производим посадку после завершения полетного задания

Три приёма из фрагмента:

wait_for_point() здесь на polling: цикл ждёт drone.point_reached() с паузой 0,1 с — SDK возвращает флаг достижения точки.
Кадр камеры запрашивается camera.get_cv_frame(timeout=1.0); если получен, он публикуется командой viewer.imshow("video", frame, fps=30). Поток video доступен по адресу rtsp://10.42.0.1:8889/video/.
В finally останавливаются и видеотракт (viewer.close(), camera.stop()), и соединение с дроном — пропускать нельзя, RTSP-сервер иначе останется работать.

mission_with_frames.py — миссия с RTSP-потоком

Полёт по «квадрату» из четырёх точек с трансляцией pioneer_camera. Программа построена на функциях: точки задаются словарями, ожидание совмещено с показом кадров:

def wait_for_point():                          # функция ожидания прилета дрона в точку
    while not drone.point_reached():           # ждем, пока дрон не достигнет заданной точки
        show_camera()                          # показываем видео с камеры во время полета
        time.sleep(0.1)                        # ставим небольшую паузу, чтобы не нагружать программу

Принятие решения о полёте вынесено в fly_through_points(), а точки хранятся в списке словарей:

def fly_through_points(points):                # функция полета по заданным точкам
    for point in points:                       # перебираем все точки из списка
        drone.go_to_local_point(               # отправляем дрон в текущую точку
            x=point["x"],                      # координата точки по оси X
            y=point["y"],                      # координата точки по оси Y
            z=point["z"],                      # координата точки по оси Z
            yaw=point["yaw"],                  # поворот по курсу в градусах
            time=3                             # время, за которое нужно достигнуть точку
        )

        wait_for_point()                       # ждем, пока дрон долетит до текущей точки


waypoints = [                                  # список точек для полетного задания
    {"x": 1, "y": 0, "z": 0.7, "yaw": 0},      # точка 1
    {"x": 1, "y": 1, "z": 0.7, "yaw": 0},      # точка 2
    {"x": 0, "y": 1, "z": 0.7, "yaw": 0},      # точка 3
    {"x": 0, "y": 0, "z": 0.7, "yaw": 0},      # возврат к начальной точке
]

Кадр здесь публикуется без fps: viewer.imshow("pioneer_camera", frame); поток доступен по адресу rtsp://10.42.0.1:8889/pioneer_camera/.

set_manual_speed.py — ручное задание скорости

Самый короткий полётный пример: две «прямые» по осям Y и X. Скорость задаётся одной командой из пяти аргументов:

try:                                          # основной код находится внутри блока try
    drone.arm()                               # включаем двигатели
    drone.takeoff()                           # взлетаем

    time.sleep(3)                             # ставим паузу на 3 секунды после взлета

    drone.set_manual_speed(0, 1, 0, 0, 2)     # летим вперед по оси Y в течение 2 секунд
                                              # vx(0) - скорость по оси "x" в м/с
                                              # vy(1) - скорость по оси "y" в м/с
                                              # vz(0) - скорость по оси "z" в м/с
                                              # yaw_rate(0) - скорость поворота по курсу
                                              # interval(2) - время действия команды в секундах

    time.sleep(2)                             # ставим паузу на 2 секунды перед следующим движением

    drone.set_manual_speed(1, 0, 0, 0, 2)     # летим вправо по оси X в течение 2 секунд
    time.sleep(2)                             # ставим паузу на 2 секунды перед посадкой

    drone.land()                              # садимся, двигатели выключатся автоматически

vx, vy, vz — скорости по осям корпуса в м/с; yaw_rate — скорость рысканья в рад/с.
interval — сколько секунд действует команда; после этого дрон зависает на месте.
Для управления относительно корпуса в полёте чаще используется set_manual_speed_body_fixed() — она применяется в aruco_flight.py и human_tracking.

Безопасность

Проверьте зону полёта, заряд, навигацию (LPS или GPS) и возможность аварийной посадки.
Каждый скрипт сажает дрон по Ctrl+C и по любой ошибке, а соединение закрывается в finally.
Примеры с ImageViewer требуют конфигурации камеры с драйвером gstreamer — см. требования на странице «Примеры».