Это проект создания управления джойстиком квадрокоптера ArDrone 2.0 c из ROS.
1. Квадрокоптер ArDrone 2.0
Parrot AR.Drone – это радиоуправляемый квадрокоптер, то есть вертолет с четырьмя несущими винтами, размещенных на выносных диагональных балках. Сам AR.Drone работает под управлением операционной системы Linux, а в качестве пульта ДУ к квадрокоптеру может выступать практически любой сенсорный смартфон и планшет на Android или iOS. Дистанция устойчивого управления по Wi-Fi – от 25 до 100 метров и зависит от помещения и погодных условий, если полеты происходят на улице.
Квадрокоптер Parrot AR.Drone 2.0 оснащен 4-мя моторами мощностью 14.5
Вт и выдающих 28 500 RPM. В редукторе используются шестерни из
нилатрона для понижения шумов, а бронзовые самосмазывающиеся подшипники
позволяют всему этому эффективно вращаться. На контроллере каждого
мотора используется 8 MIPS AVR CPU, а сам контроллер влагоустойчив.
Максимальная скорость полета — 18 км/ч
На борту обновленной версии квадрокоптера установлены 2 видеокамеры:
Фронтальная HD камера выдает 720p, 30 fps с углом объектива в 92 градуса.
Нижняя QVGA камера (320х240), 60 fps с углом объектива 64 градуса. Её
AR.Drone так же использует для замерения горизонтальной скорости.
«Мозги» дрона представляют из себя 1GHz ARM Cortex A8 процессор с 800
MHz DSP TMS320DMC64x для видео. 1Гбит DDR2 RAM на 200MHz. И управляется
это всё с помощью Linux 2.6.32. Соединение с «пультом» управления (коим
являются iOS и Android девайсы) происходит по WiFi. Так что коптер
несет на себе WiFi точку.
Ориентация в пространстве происходит за счет 3-х осевого гироскопа, 3-х
осевого акселерометра, 3-х осевого магнитометра (магнитный компас),
датчика давления и ультразвукового высотомера
Технические характеристики
Видео возможности:
- HD Видеокамера реального времени: 720p 30fps
- Широкоугольная линза: 92 градуса
- H264 формат кодирования видео
- Видео передается и записывается на устройство управления или на usb-накопитель
- Захват и сохранение изображений в JPEG (720p)
Технические характеристики
- 3х секционная литий-полимерная(LiPo) батарея 1,000 mAH
- Пропеллеры специальной формы для быстрого маневрирования
- 4 бесщеточных мотора, 14.5 Ватт и скоростью вращения 28,500 в минуту
- Малошумные шестерни Nylatron
- Автоматическая остановка всех винтов при контакте с препятствием
- Полное программное управление моторами
- Устойчивый к попаданию воды контроллер мотора
- Специальная подвеска плат управления, гасящая нагрузки
- Вес: 380г с корпусом для полетов на улице, 420г - с корпусом для полета в помещении
- Части вертолета сделаны из износоустойчивого и ударопрочного пластика
- Любая часть вертолета заменяется с помощью специальных инструментов
Электроника и датчики
- Процессор 16MHz 32 bit ARM Cortex A8 с 800MHz video DSP TMS320DMC64x
- Память 16bit DDR2 RAM на 200MHz
- Контроллеры моторов: 8 MIPS AVR CPU
- Wi-Fi b/g/n
- 3х осевой акселлерометр
- 3х осевой гироскоп с углом вращения 2000 градусов/сек
- Барометрический датчик с точностью +/- 10 Па (80см над уровнем моря)
- 60 fps вертикальная QVGA камера для измерения горизонтальной скорости
- 3х осевой магнитометр с точностью до 6 градусов
- Ультрозвуковые датчики для измерения высоты полета
- Операционная система Linux 2.6.32
AR.Drone это непросто квадрокоптер, а квадрокоптер с задумкой под идею дополненной реальности (Augmented Reality Drone). Для него есть игровые приложения дополненной реальности, а еще у него открытый API.
Из-за низкой стоимости, большого количества качественных сенсоров, а так же благодаря открытому API, AR.Drone стал популярной платформой для научных экспериментов и образовательных целей. Он применяется в работах по автоматическому управлению, обучению AI, автономному видеонаблюдению, взаимодействию человек-машина, и т.д.
У меня возникло желание организовать голосовое управление AR.Drone из ROS, чтобы в перспективе он работал в паре с роботом Turtlebot. Но для начала необходимо для подстраховки сделать Ardrone управляемым с удобного устройства. Я решил выбрать джойстик.
2. Драйвер квадрокоптера ArDrone 2.0 для ROS
Ardrone_autonomy является ROS драйвером для квадрокоптера Parrot
ArDrone. Поддерживает квадрокоптеры ArDrone 1.0 и ArDrone 2.0. Этот
пакет является ответвлением пакета ArDrone Brown. Пакет позволяет
получать сообщения с датчиков ArDrone, получать изображения с камер,
управлять движением квадрокоптера и свечением светодиодов.
Установка пакета ArDrone autonomy. Сначала клонируем код в директорию пакетов (ROS_PACKAGE_PATH — у меня ros_pkgs)
$ cd ~/ros_pkgs $ git clone https://github.com/AutonomyLab/ardrone_autonomy.git$ rosstack profile && rospack profile $ roscd ardrone_autonomy
Далее — компиляция ArDrone SDK 2.0. Запускаем скрипт build_sdk.sh
$ ./build_sdk
Если компиляция прошла успешно — проверяем содержимое каталога lib
$ ls ./lib
Должно быть так:libavcodec.a libavformat.a libpc_ardrone_notool.a libvlib.a
libavdevice.a libavutil.a libsdk.a
libavfilter.a libpc_ardrone.a libswscale.a
И сборка пакета
rosmake ardrone_autonomy
Для запуска драйвера
rosrun ardrone_autonomy ardrone_driver
Cписок тем для публикации данных драйвера ardrone_autonomy
- /ardrone/bottom/camera_info
- /ardrone/bottom/image_raw
- /ardrone/bottom/image_raw/compressed
- /ardrone/bottom/image_raw/compressed/parameter_descriptions
- /ardrone/bottom/image_raw/compressed/parameter_updates
- /ardrone/bottom/image_raw/theora
- /ardrone/bottom/image_raw/theora/parameter_descriptions
- /ardrone/bottom/image_raw/theora/parameter_updates
- /ardrone/camera_info
- /ardrone/front/camera_info
- /ardrone/front/image_raw
- /ardrone/front/image_raw/compressed
- /ardrone/front/image_raw/compressed/parameter_descriptions
- /ardrone/front/image_raw/compressed/parameter_updates
- /ardrone/front/image_raw/theora
- /ardrone/front/image_raw/theora/parameter_descriptions
- /ardrone/front/image_raw/theora/parameter_updates
- /ardrone/image_raw
- /ardrone/image_raw/compressed
- /ardrone/image_raw/compressed/parameter_descriptions
- /ardrone/image_raw/compressed/parameter_updates
- /ardrone/image_raw/theora
- /ardrone/image_raw/theora/parameter_descriptions
- /ardrone/image_raw/theora/parameter_updates
- /ardrone/imu
- /ardrone/land
- /ardrone/mag
- /ardrone/navdata
- /ardrone/reset
- /ardrone/takeoff
- /cmd_vel
- /tf
Список сервисов драйвера ardrone_autonomy
- /ardrone/bottom/image_raw/compressed/set_parameters
- /ardrone/bottom/image_raw/theora/set_parameters
- /ardrone/bottom/set_camera_info
- /ardrone/flattrim
- /ardrone/front/image_raw/compressed/set_parameters
- /ardrone/front/image_raw/theora/set_parameters
- /ardrone/front/set_camera_info
- /ardrone/image_raw/compressed/set_parameters
- /ardrone/image_raw/theora/set_parameters
- /ardrone/imu_recalib
- /ardrone/setcamchannel
- /ardrone/setflightanimation
- /ardrone/setledanimation
- /ardrone/togglecam
- /ardrone_driver/get_loggers
- /ardrone_driver/set_logger_level
Информацию, полученную от квадрокоптера, драйвер публикует в тему ardrone/navdata. Тип сообщения ardrone_autonomy::Navdata
Предоставляется следующая информация
header: ROS message header
- batteryPercent: оставшегося заряда батареи дрона (%)
- state: статус ArDropne
0: не определен 1: Inited 2: на земле 3,7: в полете 4: Hovering 5: Test (?) 6: не включен 8: Landing 9: Looping (?)
- rotx: левый / правый наклон в градусах (поворот вокруг оси X)
- roty: Вперед / назад, наклон в градусах (поворот вокруг оси Y)
- rotz: Ориентация в градусах (поворот вокруг оси Z)
- magX, magY, magZ: магнитометра (только AR-Drone 2,0 )
- pressure: давление воспринимается барометр дрона (только AR-Drone 2,0 )
- temp : температура воспринимается датчиком дрона
- wind_speed: Расчетная скорость ветра (только AR-Drone 2,0 )
- wind_angle: Расчетный угол ветра (только AR-Drone 2,0 )
- wind_comp_angle: Предполагаемый угол компенсации ветра (только AR-Drone 2,0 )
- altd: Расчетная высота (мм)
- vx, vy, vz: Линейная скорость (мм / с)
- ax, ay, az: линейное ускорение (G)
- tm: Timestamp из данных, возвращаемых Drone
В экспериментальную тему Base публикуются сообщения типа sensor_msg/Imu, выдающие показания линейного ускорения, угловой скорости и ориентации устройчтва по осям x, y, z.
Камеры
Оба AR-Drone 1,0 и 2,0 оснащен двумя камерами. Одна фронтальная камера направлена вперед и одна вертикальную камеру вниз. Драйвер ardrone_driver создает три темы ardrone/image_raw, ardrone/front/image_raw and ardrone/bottom/image_raw. В каждую из этих тем публикуются сообщения типа image_transport
Для вывода на камеру (текущую)
rosrun image_view image_view image:=/ardrone/image_raw
или на конкретную (допустим front)
rosrun image_view image_view image:=/ardrone/front/image_raw
Отправка команд для AR-Drone
Взлет — отправка пустого сообщения в тему ardrone/takeoff
Посадка - отправка пустого сообщения в тему ardrone/land
Сборос параметров(аварийная остановка) - отправка пустого сообщения в тему ardrone/reset
rostopic pub /ardrone/land std_msgs/Empty
После взлета для управления движением ArDrone необходимо посылать сообщения типа geometry_msgs::Twist в тему cmd_vel
-Linear.x: двигаться назад
+ Linear.x: двигаться вперед
-Linear.y: переместить вправо
+ Linear.y: движение влево
-Linear.z: двигаться вниз
+ Linear.z: двигаться вверх
-Angular.z: повернуть налево
+ Angular.z: повернуть направо
Диапазон для каждого компонента должно быть от -1,0 до 1,0. Максимальный диапазон может быть настроен с помощью ROS параметры обсуждаются далее в этом документе. Публикация "0" значение для всех компонентов сделает гул держать зависания.
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}'
Светодиодные анимации
Вызов службы ardrone/setledanimation будет вызывать выполнение одной из 14 предопределенных светодиодной анимаций для ArDrone.
Параметры
- uint8 типов : тип анимации, который является число в диапазоне [0 .. 13];
- float32 частоты : частота анимации в Гц;
- uint8 продолжительность : продолжительность анимации в секундах.
Тип параметра анимации:
- BLINK_GREEN_RED;
- BLINK_GREEN;
- BLINK_RED;
- BLINK_ORANGE;
- SNAKE_GREEN_RED;
- FIRE;
- STANDARD;
- RED;
- GREEN;
- RED_SNAKE;
- BLANK;
- LEFT_GREEN_RIGHT_RED;
- LEFT_RED_RIGHT_GREEN;
- BLINK_STANDARD.
Эти анимации можно протестировать в командной строке, например
rosservice call /ardrone/setledanimation 1 4 5
Полетные анимации
Вызов службы ardrone/setflightanimation будет выполнять одну из 20 предопределенных полетных анимаций (полетных фигур) для ArDrone. Параметры:
uint8 типов : тип полета анимация, число в диапазоне [0 .. 19]
uint16 продолжительность : продолжительность анимации. Используйте 0 для длительности по умолчанию (рекомендуется)
- ARDRONE_ANIM_PHI_M30_DEG;
- ARDRONE_ANIM_PHI_30_DEG;
- ARDRONE_ANIM_THETA_M30_DEG;
- ARDRONE_ANIM_THETA_30_DEG;
- ARDRONE_ANIM_THETA_20DEG_YAW_200DEG;
- ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG;
- ARDRONE_ANIM_TURNAROUND;
- ARDRONE_ANIM_TURNAROUND_GODOWN;
- ARDRONE_ANIM_YAW_SHAKE;
- ARDRONE_ANIM_YAW_DANCE;
- ARDRONE_ANIM_PHI_DANCE;
- ARDRONE_ANIM_THETA_DANCE;
- ARDRONE_ANIM_VZ_DANCE;
- ARDRONE_ANIM_WAVE;
- ARDRONE_ANIM_PHI_THETA_MIXED;
- ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED;
- ARDRONE_ANIM_FLIP_AHEAD;
- ARDRONE_ANIM_FLIP_BEHIND;
- ARDRONE_ANIM_FLIP_LEFT;
- ARDRONE_ANIM_FLIP_RIGHT.
Эти анимации можно протестировать в командной строке, например
rosservice call /ardrone/setflightanimation 1 0
Полетные анимации можно запустить во время полета ArDrone.
Можно попробовать поуправлять ArDrone и с клавиатуры
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Но я бы не советовал - управлять очень трудно - пара жестких падений квадрокоптера заставила меня отказаться от этой идеи.
Подключаем джойстик
У меня имеелся в наличии джойстик Defender Gamne Racer X7
Defender Game Racer X7 имеет 12 кнопок (включая D-Pad и 2 аналоговых джойстика), а также кнопки Turbo, Clear и Home. Устройство поддерживает вибрационную обратную связь, работающую при помощи 2-х вибромоторов. Подключение к компьютеру производится через интерфейс USB. Джойстик может работать в двух режимах, один из которых HID-устройство, другой - контроллер XBOX360 . Переключение производится с помощью кнопки Mode.
Подключаем джойстик к компьютеру с Linux.
$ ls /dev/input
Джойстик устройства называются по JSX, у меня было наш джойстик js0. Удостоверимся, что джойстик работает.
$ sudo jstest /dev/input/js0
img
На сервере параметров устанавливаем параметр joy_node/dev, где указываем порт подключения нашего джойстика
$ rosparam set joy_node/dev "/dev/input/jsX0"
И запускаем узел joy_node из пакета joy
$ rosrun joy joy_node
И смотрим сообщения, публикуемые в тему joy
img
Создаем новый ROS пакет
$ roscreate-pkg vp_ardrone1 rospy std_msgs ardrone_autonomy joy
Устанавливаем зависимости пакета
$ rosdep install vp_ardrone1
Собираем пакет
$ rosmake vp_ardrone1
Теперь нам необходимо написать скрипт, создающий узел, который будет получать сообщения из темы joy и отправлять команды управления квадрокоптеру Ardrone. Скрипт написан на python.
С помощью джойстика выполняем следующие команды
- взлет,
- посадка,
- движение (вверх, вниз, влево, вправо, вверх, вниз, поворот) со скоростью отклонения джойстиков,
- зависание,
- светодиодные анимации 0-13,
- полетные анимации 0-14 из 19 ,кроме сальто(для включения сальто поменяйте строку num2=min(num2+1,14) на num2=min(num2+1,18).
Текущие значение led-анимации и flight-анимации хранятся на сервере параметров (параметры joystick_num1 и joystick_num2). Последние значения данных, отправляемых в тему cmd_vel также хранятся на сервере параметров (параметр ). Вот его содержимое (nodes/ros_ardrone1_joystick_to_move.py).
#!/usr/bin/env python
#-*-coding:utf-8 -*-
import roslib; roslib.load_manifest('vp_ardrone1')
import rospy
import subprocess
import shlex
import time
from ardrone_autonomy.msg import *
from ardrone_autonomy.srv import *
from sensor_msgs.msg import Joy
from std_msgs.msg import String
from std_msgs.msg import Empty
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
def controller(data):
# проверка takeoff - кнопка start/10
if(data.buttons[7]==1): # взлет
rospy.loginfo("взлет")
pub1=rospy.Publisher('ardrone/takeoff', Empty)
pub1.publish()
time.sleep(1)
elif(data.buttons[6]==1): # посадка- кнопка back/9
rospy.loginfo("посадка")
pub1=rospy.Publisher('ardrone/land', Empty)
pub1.publish()
time.sleep(1)
elif(data.buttons[1]==1): # следующая led анимация - кнопка B2
num1=rospy.get_param("joystick_num1")
num1=min(num1+1,13) # 0...13
rospy.set_param("joystick_num1",num1)
serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim)
res1=serv1(num1,1,5);
rospy.loginfo("next led анимация"+str(num1))
rospy.loginfo(res1)
time.sleep(1)
elif(data.buttons[0]==1): # предыдущая led анимация - кнопка A1
num1=rospy.get_param("joystick_num1")
num1=max(num1-1,0) # 0...13
rospy.set_param("joystick_num1",num1)
serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim)
res1=serv1(num1,1,5);
rospy.loginfo("prev led анимация"+str(num1))
rospy.loginfo(res1)
time.sleep(1)
elif(data.buttons[5]==1): # завис - кнопка RB6
pub3=rospy.Publisher('cmd_vel', Twist)
odom=Twist()
odom.linear.x=0.0
odom.linear.y=0.0
odom.linear.z=0.0
odom.angular.x=0.0
odom.angular.y=0.0
odom.angular.z=0.0
pub3.publish(odom)
rospy.loginfo("завис!!!!")
time.sleep(1)
elif(data.buttons[3]==1): # следующая полетная анимация - кнопка Y4
num2=rospy.get_param("joystick_num2")
num2=min(num2+1,14) # 0...18 (ограничено flip)
rospy.set_param("joystick_num2",num2)
serv1=rospy.ServiceProxy('ardrone/setflightanimation',FlightAnim)
res1=serv1(num2,0);
rospy.loginfo("next полетная анимация"+str(num2))
rospy.loginfo(res1)
time.sleep(1)
elif(data.buttons[2]==1): # предыдущая полетная анимация - кнопка X3
num2=rospy.get_param("joystick_num2")
num2=max(num2-1,0) # 0...18 (ограничено flip)
rospy.set_param("joystick_num2",num2)
serv1=rospy.ServiceProxy('ardrone/setflightanimation',FlightAnim)
res1=serv1(num2,0);
rospy.loginfo("prev - полетная анимация"+str(num2))
rospy.loginfo(res1)
time.sleep(1)
elif(data.axes[0]!=0.0 or data.axes[1]!=0.0 or data.axes[3]!=0.0 or data.axes[4]!=0.0): # управление
pub3=rospy.Publisher('cmd_vel', Twist)
joistick_prev_odom=rospy.get_param("joystick_prev_odom")
odom=Twist()
odom.linear.x=data.axes[1]
odom.linear.y=data.axes[0]
odom.linear.z=data.axes[4]
odom.angular.x=0.0
odom.angular.y=0.0
odom.angular.z=data.axes[3]
pub3.publish(odom)
joistick_prev_odom[0]=data.axes[0]
joistick_prev_odom[1]=data.axes[1]
joistick_prev_odom[2]=data.axes[2]
joistick_prev_odom[3]=data.axes[3]
joistick_prev_odom[4]=data.axes[4]
joistick_prev_odom[5]=data.axes[5]
rospy.set_param("joystick_prev_odom",joistick_prev_odom)
rospy.loginfo("движение!!!!")
else: # останов джойстиками
joistick_prev_odom=rospy.get_param("joystick_prev_odom")
odom=Twist()
if(joistick_prev_odom[0]!=data.axes[0] or joistick_prev_odom[1]!=data.axes[1] or
joistick_prev_odom[3]!=data.axes[3] or joistick_prev_odom[4]!=data.axes[4]):
pub3=rospy.Publisher('cmd_vel', Twist)
odom.linear.x=0.0
odom.linear.y=0.0
odom.linear.z=0.0
odom.angular.x=0.0
odom.angular.y=0.0
odom.angular.z=0.0
pub3.publish(odom)
joistick_prev_odom[0]=0.0
joistick_prev_odom[1]=0.0
joistick_prev_odom[2]=0.0
joistick_prev_odom[3]=0.0
joistick_prev_odom[4]=0.0
joistick_prev_odom[5]=0.0
rospy.set_param("joystick_prev_odom",joistick_prev_odom)
rospy.loginfo("останов по джойстику!!!!")
#
#rospy.loginfo(data.axes)
#rospy.loginfo(data.buttons)
def listener():
rospy.init_node('joyctick')
if not rospy.has_param("joystick_num1"):
rospy.set_param("joystick_num1",0)
if not rospy.has_param("joystick_num2"):
rospy.set_param("joystick_num2",0)
if not rospy.has_param("joystick_prev_odom"):
rospy.set_param("joystick_prev_odom",[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
sub = rospy.Subscriber("joy",Joy,controller)
rospy.spin()
if __name__ == '__main__':
listener()
Запускаем
$ rosrun vp_ardrone1 ros_ardrone1_joystick_to_move.py
Вы не можете скачивать файлы с нашего сервера