Название проекта — «Вот и встретились два квадрокоптера» (два Ardrone 2.0 ищут друг друга и встречаются). Но так как целый только один, а другой не работает, поиском друга будет заниматься только рабочий собрат, другой будет перемещаться с помощью человека.
Квадрокоптер управляется из ROS.
Наносим на корпус квадрокоптера идущие в комплекте метки
Создаем в 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
Скрипт 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 поставлялся с кожухом для корпуса и метками трех цветовых раскрасок
Для того, чтобы пакет 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
Смотрим
Теперь пишем скрипт на 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