CraftDuino v2.0
  • - это CraftDuino - наш вариант полностью Arduino-совместимой платы.
  • CraftDuino - настоящий конструктор, для очень быстрого прототипирования и реализации идей.
  • Любая возможность автоматизировать что-то с лёгкостью реализуется с CraftDuino!
Просто добавьте CraftDuino!

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

Название проекта — «Вот и встретились два квадрокоптера» (два 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)
Для имеющихся у меня сине-оранжевых меток
<param name="enemy_colors" value="3"></param>

Этот параметр необходимо записать к командный файл запуска пакета 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 height<MIN_HEIGHT:
      search_z_direction=1;
      
    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
    if search_z_direction==1:
      odom.linear.z=0.1;
    elif search_z_direction==2:
      odom.linear.z=-0.1;

    pub1.publish(odom)
    rospy.get_param("search_z_direction",search_z_direction)
    #rospy.loginfo("===="+str(search_z_direction))

    return


def controller(data):

    # видны маркеры? 
    if(data.tags_count>0):  # маркер виден
        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

<launch>
  <!-- для джойстика  -->
  <param name="joy_node" value="/dev/input/js0"></param>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <!-- управление ardrone с помощью джойстика  -->
  <node name="" pkg="vp_ardrone1" type="ros_ardrone1_joystick_to_move.py" />
  <!-- поиск квадрокоптера по меткам  -->
  <node name="" pkg="vp_ardrone1" type="ros_get_navdata_marker2.py" />
</launch>


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

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

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

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.