Вот и встретились два квадрокоптера — 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)
Для имеющихся у меня сине-оранжевых меток


Этот параметр необходимо записать к командный файл запуска пакета 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

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

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение