Автономный полет
Модуль simple_offboard
пакета clover
предназначен для упрощенного программирования автономного полета дрона (режим OFFBOARD
). Он позволяет устанавливать желаемые полетные задачи и автоматически трансформирует систему координат.
simple_offboard
является высокоуровневым способом взаимодействия с полетным контроллером. Для более низкоуровневой работы см. mavros.
Основные сервисы – get_telemetry
(получение телеметрии), navigate
(полет в заданную точку по прямой), navigate_global
(полет в глобальную точку по прямой), land
(переход в режим посадки).
Использование из языка Python
Для использования сервисов, необходимо создать объекты-прокси к ним. Используйте этот шаблон для вашей программы:
import rospy
from clover 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_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)
Неиспользуемые функции-прокси можно удалить из кода.
Описание API
СОВЕТ
Незаполненные числовые параметры устанавливаются в значение 0.
get_telemetry
Получить полную телеметрию коптера.
Параметры:
frame_id
– система координат для значенийx
,y
,z
,vx
,vy
,vz
. Пример:map
,body
,aruco_map
. Значение по умолчанию:map
.
Формат ответа:
frame_id
– система координат;connected
– есть ли подключение к FCU;armed
– состояниеarmed
винтов (винты включены, если true);mode
– текущий полетный режим;x, y, z
– локальная позиция коптера (м);lat, lon
– широта, долгота (градусы), необходимо наличие GPS;alt
– высота в глобальной системе координат (стандарт WGS-84, не AMSL!), необходимо наличие GPS;vx, vy, vz
– скорость коптера (м/с);pitch
– угол по тангажу (радианы);roll
– угол по крену (радианы);yaw
– угол по рысканью (радианы);pitch_rate
– угловая скорость по тангажу (рад/с);roll_rate
– угловая скорость по крену (рад/с);yaw_rate
– угловая скорость по рысканью (рад/с);voltage
– общее напряжение аккумулятора (В);cell_voltage
– напряжение аккумулятора на ячейку (В).
СОВЕТ
Недоступные по каким-то причинам поля будут содержать в себе значения NaN
.
Вывод координат x
, y
и z
коптера в локальной системе координат:
telemetry = get_telemetry()
print(telemetry.x, telemetry.y, telemetry.z)
Вывод высоты коптера относительно карты ArUco-меток:
telemetry = get_telemetry(frame_id='aruco_map')
print(telemetry.z)
Проверка доступности глобальной позиции:
import math
if not math.isnan(get_telemetry().lat):
print('Global position is available')
else:
print('No global position')
Вывод текущей телеметрии (командная строка):
rosservice call /get_telemetry "{frame_id: ''}"
navigate
Прилететь в обозначенную точку по прямой.
Параметры:
x
,y
,z
– координаты (м);yaw
– угол по рысканью (радианы);yaw_rate
– угловая скорость по рысканью (применяется при установке yaw вNaN
) (рад/с);speed
– скорость полета (скорость движения setpoint) (м/с);auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);frame_id
– система координат, в которой заданыx
,y
,z
иyaw
(по умолчанию:map
).
СОВЕТ
Для полета без изменения угла по рысканью достаточно установить yaw
в NaN
(значение угловой скорости по умолчанию – 0).
Взлет на высоту 1.5 м со скоростью взлета 0.5 м/с:
navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True)
Полет по прямой в точку 5:0 (высота 2) в локальной системе координат со скоростью 0.8 м/с (рысканье установится в 0):
navigate(x=5, y=0, z=3, speed=0.8)
Полет в точку 5:0 без изменения угла по рысканью (yaw
= NaN
, yaw_rate
= 0):
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')
Полет влево на 2 м относительно последней целевой точки полета дрона:
navigate(x=0, y=2, z=0, speed=1, frame_id='navigate_target')
Повернуться на 90 градусов по часовой:
navigate(yaw=math.radians(-90), frame_id='body')
Полет в точку 3:2 (высота 2) в системе координат маркерного поля со скоростью 1 м/с:
navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map')
Вращение на месте со скоростью 0.5 рад/c (против часовой):
navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body')
Полет вперед 3 метра со скоростью 0.5 м/с, вращаясь по рысканью со скоростью 0.2 рад/с:
navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body')
Взлет на высоту 2 м (командная строка):
rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"
СОВЕТ
При программировании миссии дрона в терминах "вперед-назад-влево-вправо" рекомендуется использовать систему координат navigate_target
вместо body
, чтобы не учитывать неточность прилета дрона в предыдущую целевую точку при вычислении следующей.
navigate_global
Полет по прямой в точку в глобальной системе координат (широта/долгота).
Параметры:
lat
,lon
– широта и долгота (градусы);z
– высота (м);yaw
– угол по рысканью (радианы);yaw_rate
– угловая скорость по рысканью (при установке yaw вNaN
) (рад/с);speed
– скорость полета (скорость движения setpoint) (м/с);auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);frame_id
– система координат, в которой заданыz
иyaw
(по умолчанию:map
).
СОВЕТ
Для полета без изменения угла по рысканью достаточно установить yaw
в NaN
(значение угловой скорости по умолчанию – 0).
Полет в глобальную точку со скоростью 5 м/с, оставаясь на текущей высоте (yaw
установится в 0, коптер сориентируется передом на восток):
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body')
Полет в глобальную точку без изменения угла по рысканью (yaw
= NaN
, yaw_rate
= 0):
navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body')
Полет в глобальную точку (командная строка):
rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"
set_position
Установить цель по позиции и рысканью. Данный сервис следует использовать при необходимости задания продолжающегося потока целевых точек, например, для полета по сложным траекториям (круговой, дугообразной и т. д.).
СОВЕТ
Для полета на точку по прямой или взлета используйте более высокоуровневый сервис navigate
.
Параметры:
x
,y
,z
– координаты точки (м);yaw
– угол по рысканью (радианы);yaw_rate
– угловая скорость по рысканью (при установке yaw в NaN) (рад/с);auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);frame_id
– система координат, в которой заданыx
,y
,z
иyaw
(по умолчанию:map
).
Зависнуть на месте:
set_position(frame_id='body')
Назначить целевую точку на 3 м выше текущей позиции:
set_position(x=0, y=0, z=3, frame_id='body')
Назначить целевую точку на 1 м впереди текущей позиции:
set_position(x=1, y=0, z=0, frame_id='body')
Вращение на месте со скоростью 0.5 рад/c:
set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5)
set_velocity
Установить скорости и рысканье.
vx
,vy
,vz
– требуемая скорость полета (м/с);yaw
– угол по рысканью (радианы);yaw_rate
– угловая скорость по рысканью (при установке yaw в NaN) (рад/с);auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);frame_id
– система координат, в которой заданыvx
,vy
,vz
иyaw
(по умолчанию:map
).
СОВЕТ
Параметр frame_id
определяет только ориентацию результирующего вектора скорости, но не его длину.
Полет вперед (относительно коптера) со скоростью 1 м/с:
set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
set_attitude
Установить тангаж, крен, рысканье и уровень газа (примерный аналог управления в режиме STABILIZED
). Данный сервис может быть использован для более низкоуровневого контроля поведения коптера либо для управления коптером при отсутствии источника достоверных данных о его позиции.
Параметры:
pitch
,roll
,yaw
– необходимый угол по тангажу, крену и рысканью (радианы);thrust
– уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ);auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);frame_id
– система координат, в которой заданyaw
(по умолчанию:map
).
set_rates
Установить угловые скорости по тангажу, крену и рысканью и уровень газа (примерный аналог управления в режиме ACRO
). Это самый низкий уровень управления коптером (исключая непосредственный контроль оборотов моторов). Данный сервис может быть использован для автоматического выполнения акробатических трюков (например, флипа).
Параметры:
pitch_rate
,roll_rate
,yaw_rate
– угловая скорость по тангажу, крену и рыканью (рад/с);thrust
– уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ).auto_arm
– перевести коптер вOFFBOARD
и заармить автоматически (коптер взлетит);
Положительное направление вращения yaw_rate
(при виде сверху) – против часовой, pitch_rate
– вперед, roll_rate
– влево.
land
Перевести коптер в режим посадки (AUTO.LAND
или аналогичный).
СОВЕТ
Для автоматического отключения винтов после посадки параметр PX4 COM_DISARM_LAND
должен быть установлен в значение > 0.
Посадка коптера:
res = land()
if res.success:
print('Copter is landing')
Посадка коптера (командная строка):
rosservice call /land "{}"
ВНИМАНИЕ
В более новых версиях PX4 коптер выйдет из режима LAND в ручной режим, если сильно перемещать стики.