Автономный полет (OFFBOARD)

Что это за модуль

Модуль simple_offboard пакета sverk позволяет программировать автономный полет коптера в режиме OFFBOARD.

Он берёт на себя сложные детали — взаимодействие с полетным контроллером и пересчёт координат между разными системами (фреймами).

Если нужно низкоуровневое управление, можно использовать mavros, но для большинства учебных задач удобнее simple_offboard.

Основные сервисы модуля:

get_telemetry — получить телеметрию (положение, высоту, скорость, напряжение и т. д.).
navigate — полететь в указанную точку по прямой.
navigate_global — полететь в глобальную точку по широте/долготе.
land — посадить коптер, перевести его в режим посадки.

Шаблон программы на Python

Чтобы пользоваться сервисами в Python, в начале программы необходимо создать «прокси»-объекты — это удобные функции, которые внутри обращаются к сервисам ROS.

import rospy
from sverk import srv
from std_srvs.srv import Trigger

rospy.init_node('flight')

get_telemetry   = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate        = rospy.ServiceProxy('navigate', srv.Navigate)
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
set_altitude    = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
set_yaw         = rospy.ServiceProxy('set_yaw', srv.SetYaw)
set_yaw_rate    = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
set_position    = rospy.ServiceProxy('set_position', srv.SetPosition)
set_velocity    = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
set_attitude    = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates       = rospy.ServiceProxy('set_rates', srv.SetRates)
land            = rospy.ServiceProxy('land', Trigger)

Функции, которые не планируете использовать, можно просто удалить из этого списка.

Кратко о главных сервисах

`get_telemetry` — получить данные о коптере

get_telemetry(frame_id='map') возвращает координаты, угол поворота, скорость, состояние моторов, напряжение и т. д.

Параметры:

frame_id — в какой системе координат вернуть положение и скорость (map, body, aruco_map и др.).

Примеры:

Вывести x, y, z в локальной системе по умолчанию:

  telemetry = get_telemetry()
  print(telemetry.x, telemetry.y, telemetry.z)

Вывести высоту над картой ArUco:

  telemetry = get_telemetry(frame_id='aruco_map')
  print(telemetry.z)

Если каких-то данных нет (например, нет GPS), соответствующие поля будут равны NaN.

`navigate` — полет в точку по прямой

navigate ставит задачу «долети до такой-то точки».

Функция не ждёт, пока коптер долетит — программа сразу идёт дальше.(programming.md)

Параметры:

x, y, z — координаты цели в выбранной системе координат (метры);
yaw — угол поворота по рысканью (радианы);
speed — скорость полета (м/с);
auto_arm — если True, коптер сам перейдёт в OFFBOARD и взлетит;
frame_id — система координат, в которой заданы x, y, z, yaw (по умолчанию map).(frames.md)

Примеры:

Взлет на 1.5 м относительно корпуса:

  navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True)

Полет в точку (5, 0, 3) в map со скоростью 0.8 м/с:
```
  navigate(x=5, y=0, z=3, speed=0.8)
```
Полет в ту же точку без изменения текущего yaw:
```
  navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan'))
```
Полет вправо относительно коптера на 3 м:
```
  navigate(x=0, y=-3, z=0, speed=1, frame_id='body')
```
Полет в точку (3, 2, 2) в системе координат маркерного поля:
```
  navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map')
```

При программировании последовательных шагов «вперёд-назад-влево-вправо» удобнее использовать фрейм navigate_target, чтобы ошибки позиционирования не накапливались.

`navigate_global` — полет по GPS-координатам

navigate_global похож на navigate, но задаёт цель в виде широты и долготы.

Параметры:

lat, lon — координаты точки [градусы];
z — высота [м];
yaw — угол по рысканью [радианы];
speed — скорость полета [м/с];
auto_arm — автоматический переход в OFFBOARD и взлёт;
frame_id — система координат по умолчанию map.

Пример — полет в точку, оставаясь на текущей высоте:

navigate_global(
    lat=55.707033,
    lon=37.725010,
    z=0,
    speed=5,
    frame_id='body'
)

Полезные сервисы

Все они работают в тех же фреймах (frame_id), что и navigate.

set_altitude(z, frame_id) — задать высоту полета независимо от текущей команды (navigate или set_position);
set_yaw(yaw, frame_id) — задать целевой угол по рысканью, не меняя текущую задачу по положению;
set_yaw_rate(yaw_rate) — задать угловую скорость вращения вокруг вертикальной оси;
set_position(x, y, z, yaw, frame_id) — задать текущую целевую позицию; удобно для сложных траекторий с частым обновлением точки;
set_velocity(vx, vy, vz, yaw, frame_id) — управление скоростями (лететь вперёд/назад/вбок с заданной скоростью);
set_attitude(roll, pitch, yaw, thrust, frame_id) — управление креном, тангажом, рысканьем и газом (аналог ручного режима);
set_rates(roll_rate, pitch_rate, yaw_rate, thrust) — управление угловыми скоростями и газом (низкоуровневый режим, пригоден для трюков);
land() — перевести коптер в режим посадки (AUTO.LAND или аналог).

Функция navigate_wait: полет в точку с ожиданием

Зачем нужна navigate_wait

Как уже отмечалось, navigate сразу возвращает управление: скрипт продолжает выполняться, даже если коптер ещё летит к цели.(programming.md)

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

Для этого удобно использовать функцию navigate_wait.

import math

def navigate_wait(
    x=0, y=0, z=0,
    yaw=float('nan'),
    speed=0.5,
    frame_id='',
    auto_arm=False,
    tolerance=0.2
):
    # 1. Отправляем команду полета в точку
    navigate(
        x=x, y=y, z=z,
        yaw=yaw,
        speed=speed,
        frame_id=frame_id,
        auto_arm=auto_arm
    )

    # 2. Ждем, пока коптер подлетит к цели достаточно близко
    while not rospy.is_shutdown():
        # Берём телеметрию в фрейме navigate_target
        telem = get_telemetry(frame_id='navigate_target')

        # Считаем расстояние до целевой точки
        distance = math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2)

        if distance < tolerance:
            # Коптер достаточно близко к точке — выходим из цикла
            break

        # Ждём немного перед следующей проверкой
        rospy.sleep(0.2)

Что делает эта функция по шагам:

Вызывает navigate с теми же параметрами, что вы передали в navigate_wait;
Переходит в цикл ожидания;
В цикле регулярно запрашивает телеметрию в системе координат navigate_target;
Считает расстояние от текущего положения до целевой точки;
Приводит в движение полет к точке;
Когда расстояние становится меньше tolerance (по умолчанию 0.2 м), цикл прерывается и функция возвращает управление.

Благодаря этому ваша программа «останавливается» на вызове navigate_wait до тех пор, пока коптер практически не достигнет нужной точки.

Почему используется фрейм navigate_target

Фрейм navigate_target — специальная система координат, центр которой совпадает с целевой точкой, заданной последним вызовом navigate.

В этой системе координат:
- целевая точка имеет координаты 𝑥=0, 𝑦=0, 𝑧=0;
- текущая позиция коптера показывает «насколько мы ещё не долетели» по каждой оси.

Поэтому расстояние до цели удобно считать именно там:

distance=𝑥2+𝑦2+𝑧2

Если это расстояние меньше tolerance, можно считать, что коптер прилетел достаточно близко к цели.

Примеры использования navigate_wait

Полёт в точку по карте маркеров

Полет в точку x=3, y=2, z=1 относительно карты маркеров aruco_map:

navigate_wait(x=3, y=2, z=1, frame_id='aruco_map')

Сценарий:

Коптер получает задачу полететь в точку (3, 2, 1) в системе координат маркерного поля.
Функция ждёт, пока коптер не приблизится к ней ближе, чем на tolerance метров (по умолчанию 0.2).
Лишь после этого управление возвращается в вашу программу, и можно выполнять следующую команду (например, посадку или полет в следующую точку).

Использование для взлёта

navigate_wait можно использовать и для простого взлёта на нужную высоту:

navigate_wait(z=1, frame_id='body', auto_arm=True)

Что происходит:

auto_arm=True — коптер переходит в OFFBOARD и взлетает;
z=1, frame_id='body' — цель: подняться на 1 м относительно своего текущего положения;
Функция ждёт, пока коптер окажется на заданной высоте с точностью до tolerance метров.

Точность прилёта (tolerance)

Параметр tolerance задаёт, насколько близко к целевой точке коптер должен подлететь, чтобы считать задачу выполненной.

Для учебных задач в помещении обычно достаточно 0.2–0.3 м;
Если коптер «зависает» и не может точно попасть в точку, можно немного увеличить tolerance;
Если нужна максимально аккуратная остановка, можно уменьшить tolerance, но это может увеличить время ожидания.

Пример с более строгим допуском:

navigate_wait(x=1, y=0, z=1, frame_id='aruco_map', tolerance=0.1)

Итог: типичный сценарий полёта

Ниже пример простого сценария, комбинирующего описанные функции:

# Взлёт на 1 м
navigate_wait(z=1, frame_id='body', auto_arm=True)

# Полет к точке над маркерным полем
navigate_wait(x=3, y=2, z=1, frame_id='aruco_map')

# Разворот на месте (продолжаем висеть)
set_yaw(yaw=math.radians(90), frame_id='body')

# Посадка
land()

Настройка камеры

Для корректной работы всех функций, связанных с компьютерным зрением (в том числе полета по ArUco-маркерам и Optical Flow) необходимо сфокусировать основную камеру, а также выставить ее расположение и ориентацию. Улучшить качество работы также может опциональная калибровка камеры.

Настройка фокуса камеры

Для успешного осуществления полетов с использованием камеры, необходимо настроить фокус камеры.

Откройте трансляцию изображения с камеры используя web_video_server.
С помощью вращения объектива камеры добейтесь максимальной резкости деталей (предпочтительно на расстоянии предполагаемой высоты полета – 2–3 м).

Расфокусированное изображение	Сфокусированное изображение

-->

Команды (OFFBOARD)

Автономный полет (OFFBOARD)

Что это за модуль

Шаблон программы на Python

Кратко о главных сервисах

`get_telemetry` — получить данные о коптере

`navigate` — полет в точку по прямой

`navigate_global` — полет по GPS-координатам

Полезные сервисы

Функция navigate_wait: полет в точку с ожиданием

Зачем нужна navigate_wait

Почему используется фрейм navigate_target

Примеры использования navigate_wait

Полёт в точку по карте маркеров

Использование для взлёта

Точность прилёта (tolerance)

Итог: типичный сценарий полёта

Настройка камеры

Настройка фокуса камеры

results matching ""

No results matching ""

Автономный полет (OFFBOARD)

Что это за модуль

Шаблон программы на Python

Кратко о главных сервисах

get_telemetry — получить данные о коптере

navigate — полет в точку по прямой

navigate_global — полет по GPS-координатам

Полезные сервисы

Функция navigate_wait: полет в точку с ожиданием

Зачем нужна navigate_wait

Почему используется фрейм navigate_target

Примеры использования navigate_wait

Полёт в точку по карте маркеров

Использование для взлёта

Точность прилёта (tolerance)

Итог: типичный сценарий полёта

Настройка камеры

Настройка фокуса камеры

results matching ""

No results matching ""

`get_telemetry` — получить данные о коптере

`navigate` — полет в точку по прямой

`navigate_global` — полет по GPS-координатам