Вот и встретились два квадрокоптера — Ardrone 2.0 ищет друга (поиск другого дрона по меткам)

Название проекта — «Вот и встретились два квадрокоптера» (два Ardrone 2.0 ищут друг друга и встречаются). Но так как целый только один, а другой не работает, поиском друга будет заниматься только рабочий собрат, другой будет перемещаться с помощью человека.

Квадрокоптер управляется из ROS.

Наносим на корпус квадрокоптера идущие в комплекте метки

4642f5ed22

531d61e02d

Создаем в ROS (версия indigo) в рабочем пространстве catkin новый пакет vp_ardrone1.

cd ~/catkin_ws/src
catkin_create_pkg vp_ardrone1 std_msgs rospy joy ardrone_autonomy

При этом сразу указываем зависимости пакета.
Пакет ardrone_autonomy — драйвер для управления Ardrone из операционной системы для роботов ROS.
Он должен быть предварительно установлен
(
Установка пакета Ardrone
cd ~/catkin_ws/src
git clone https://github.com/AutonomyLab/ardrone_autonomy.git -b indigo-devel
cd ~/catkin_ws
catkin_make

)

joy — драйвер ROS для геймпадов. Я использовал геймпад Defender Game Racer X7 для взлета и посадки Ardrone 2.0

3af3ff4223

Скрипт nodes/ros_ardrone1_joystick_to_move.py — (я взял из предыдущего проекта Управление квадрокоптером Ardone 2.0 c с помощью джойстика из ROS) для взлета и посадки
Взлет — клавиша START/10
Посадка — клавиша BACK/9

Теперь о поиске по меткам. Я думал использовать компьютерное зрение для поиска меток, но оказалось, что можно сделать это гораздо проще.
Информацию, полученную от квадрокоптера, драйвер публикует в тему ardrone/navdata. Тип сообщения ardrone_autonomy::Navdata

Среди большого количества предоставляемой информации есть следующие параметры

tags_count: 0
tags_type: []
tags_xc: []
tags_yc: []
tags_width: []
tags_height: []
tags_orientation: []
tags_distance: []

Это количество найденных меток, координаты, размеры, ориентация и дистанция до меток

Ardrone 2.0 Classic поставлялся с кожухом для корпуса и метками трех цветовых раскрасок

aa541c9c40
dfa0a83ea5
7cdf602389

Для того, чтобы пакет ardrone_autonomy фиксировал метку и ее координаты с помощью фронтальной камеры, необходимо установить значение параметра пакета enemy_colors в соответствующее значение (1,2 или 3)
Для имеющихся у меня сине-оранжевых меток

Этот параметр необходимо записать к командный файл запуска пакета ardrone_autonomy
Запустим пакет ardrone_autonomy и посмотрим как определяются метки
1 терминал
roscore

2 терминал
roslaunch ardrone_autonomy ardrone1

3 терминал (выводим изображение с фронтальной камеры)
rosrun image_view image_view image:=/ardrone/front/image_raw

4 терминал (смотрим сообщения публикуемые в тему ardrone/navdata)
rostopic echo /ardrone/navdata

Смотрим

9de33604d2

Теперь пишем скрипт на python, который должен выполнять следующие действия
— получать сообщения из темы /ardrone/navdata
— при отсутствие обнаружения меток дрон движется по кругу, затем вверх, по кругу,… вверх до высоты MAX_HEIGHT, затем вниз, по кругу,… вниз до MIN_HEIGHT
— при обнаружении метки движение к ней до минимальной дистанции

Создаем файл ros_get_navdata_marker2.py

cd nodes
touch ros_get_navdata_marker2.py
chmod +x ros_get_navdata_marker2.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
from ardrone_autonomy.msg import Navdata

MIN_HEIGHT=500
MAX_HEIGHT=1500

def go(x,y,w,h,d):
odom=Twist()
pub1=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
xx=" ";yy=" ";zz=" ";k=0
if y[0]<200: yy="up";odom.linear.z=0.1 elif y[0]>600:
yy="down";odom.linear.z=-0.1
if x[0]<300: xx="left";k=1;odom.angular.z=0.1 elif x[0]>600:
xx="right";k=1;odom.angular.z=-0.1
if d[0]>250.0:
odom.linear.x=0.1;zz="forward"
elif d[0]<100.0: odom.linear.x=-0.1;zz="back" #serv1=rospy.ServiceProxy('ardrone/setledanimation', LedAnim) #res1=serv1(5.0,2,1.0); #rospy.sleep(5.0) #serv2=rospy.ServiceProxy('ardrone/setflightanimation',FlightAnim) #res2=serv2(11,0); #rospy.sleep(5.0) #pub2=rospy.Publisher('ardrone/land', Empty) #pub2.publish() #rospy.sleep(5.0) pass rospy.loginfo("x="+str(x[0])+" y="+str(y[0])+" d="+str(d[0])) rospy.loginfo(" "+xx+" "+yy+" "+zz) #if k>0:
pub1.publish(odom)
return

def search_xy(rotZ,time1):
odom=Twist()
pub1=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.1
pub1.publish(odom)
if abs(rotZ)<10.0 and time1>3000:
time1=8000
rospy.loginfo(rotZ)
return time1

def search_z(height):
odom=Twist()
pub1=rospy.Publisher('cmd_vel', Twist)
search_z_direction=rospy.get_param("search_z_direction")
if height>MAX_HEIGHT:
search_z_direction=2;
elif height0): # маркер виден
rospy.set_param("last_tags_xc",data.tags_xc)
rospy.set_param("last_tags_yc",data.tags_yc)
rospy.set_param("last_tags_width",data.tags_width)
rospy.set_param("last_tags_height",data.tags_height)
rospy.set_param("last_tags_distance",data.tags_distance)
go(data.tags_xc,data.tags_yc,data.tags_width,data.tags_height,data.tags_distance)
rospy.set_param("time_no_view_marker",0)
rospy.loginfo("view"+str(data.tags_distance))
else: # маркеры не видны
time_no_view_marker=rospy.get_param("time_no_view_marker")
if time_no_view_marker<1000: last_tags_xc=rospy.get_param("last_tags_xc") last_tags_yc=rospy.get_param("last_tags_yc") last_tags_width=rospy.get_param("last_tags_width") last_tags_height=rospy.get_param("last_tags_height") last_tags_distance=rospy.get_param("last_tags_distance") go(last_tags_xc,last_tags_yc,last_tags_width,last_tags_height,last_tags_distance) rospy.loginfo("?????????????????"+str(last_tags_distance)) elif time_no_view_marker<8000: time_no_view_marker=search_xy(data.rotZ,time_no_view_marker) rospy.loginfo("?????????????????????") elif time_no_view_marker<11000: search_z(data.altd) rospy.loginfo("&&&&&&&&&&&&&") else: rospy.loginfo("@@@@@@@@@@@@@@@@@@@@@@@@") time_no_view_marker=1000; rospy.set_param("time_no_view_marker",time_no_view_marker+100) def listener(): rospy.init_node('view_marker') if not rospy.has_param("time_no_view_marker"): rospy.set_param("time_no_view_marker",1000) if not rospy.has_param("last_tags_count"): rospy.set_param("last_tags_count",0) if not rospy.has_param("last_tags_xc"): rospy.set_param("last_tags_xc",0) if not rospy.has_param("last_tags_yc"): rospy.set_param("last_tags_yc",0) if not rospy.has_param("last_tags_width"): rospy.set_param("last_tags_width",0) if not rospy.has_param("last_tags_height"): rospy.set_param("last_tags_height",0) if not rospy.has_param("last_tags_distance"): rospy.set_param("last_tags_distance",0) if not rospy.has_param("search_z_direction"): rospy.set_param("search_z_direction",1) if not rospy.has_param("search_rot_z"): rospy.set_param("search_rot_z",0) sub = rospy.Subscriber("/ardrone/navdata",Navdata,controller) rospy.spin() if __name__ == '__main__': listener()

Пишем командный файл для запуска
— пакета joy — публикация событий геймпада
— ros_ardrone1_joystick_to_move.py (управление ardrone с помощью джойстика — используем для взлета и посадки)
— ros_get_navdata_marker2.py (поиск ardrone по меткам)

Вот содержимое командного файла launch1.launch








И запуск
1 терминал
roscore

2 терминал
roslaunch ardrone_autonomy ardrone1

3 терминал
roslaunch vp_ardrone1 launch1.launch

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

Ваш e-mail не будет опубликован. Обязательные поля помечены *

Можно использовать следующие HTML-теги и атрибуты: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <s> <strike> <strong>