Управление квадрокоптером ArDrone 2.0 через ROS с помощью джойстика

Это проект создания управления джойстиком квадрокоптера 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. Джойстик может работать в 2-х режимах, один из которых 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

    Прикрепленные файлы:

    Добавить комментарий

    Ваш адрес email не будет опубликован.