Entorno de prácticas del Curso Superior Universitario en

Transcripción

Entorno de prácticas del Curso Superior
Universitario en Programación de drones
Universidad Rey Juan Carlos
Alberto Martín Florido
José María Cañas Plaza
Eduardo Perdices
1
Índice
1. Plataforma JdeRobot
4
1.1. Middleware de comunicaciones: ICE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
1.2. Simulador Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2. Interfaces JdeRobot relevantes para el curso
7
2.1. camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
2.2. Pose3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.3. cmd_vel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
2.4. ardrone_extra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2.5. motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
3. Componentes relevantes para el curso
12
3.1. Servidor cameraserver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
3.2. Servidor ardrone_server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
3.3. Componente introrob_py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
3.4. Componente introrob_qt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
4. Práctica 1: Percepción visual a bordo del drone con cámara
17
4.1. Configuración del entorno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
4.1.1. Componente cameraserver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
4.1.2. Componente colorFilter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
4.2. Programando la detección de un objeto . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
5. Práctica 2: Navegación local por posición
23
23
5.1.1. Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
5.1.2. Componente controlPID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
5.2. Implementando un algoritmo de navegación local . . . . . . . . . . . . . . . . . . . . . . . .
26
6. Práctica 3: Seguimiento de objetos con AR.Drone
28
28
6.1.1. Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
6.1.2. Componente introrob_qt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
6.1.3. Componente catchTheTurtle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
2
30
6.2. Desarrollando un algoritmo para el seguimiento de un robot terrestre . . . . . . . . . . . . .
32
7. Práctica 4: Tu cuadricóptero sigue una carretera desde el aire
34
34
7.1.1. Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
34
34
7.1.3. Componente introrob_py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
7.2. Programando la detección de la carretera y el algoritmo de control . . . . . . . . . . . . . .
35
8. Práctica 5: Aterrizando encima de un coche
37
8.1. Configurando el entorno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
37
8.2. Programando un autómata de estado finito . . . . . . . . . . . . . . . . . . . . . . . . . . .
38
8.2.1. Creando un autómata de estado finito en visualLander . . . . . . . . . . . . . . . .
39
8.3. Programando la percepción de la baliza . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
8.4. ¿Cómo hacerlo más divertido? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
9. Práctica 6: Laberinto con flechas
43
43
9.2. Programando la detección de objetos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
44
9.3. Programando el control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
45
45
10.Práctica 7: Rescate de personas
46
46
10.2. Navegar para explorar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
10.3. Detección de personas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
48
3
En este documento se describen los diferentes ingredientes del entorno software para la programación
de los cuadricópteros durante las prácticas del curso.
1.
Plataforma JdeRobot
JdeRobot1 es una plataforma de software libre para el desarrollo de aplicaciones con robots, visión
artificial y domótica. Este proyecto ha sido desarrollado y está mantenido por el Grupo de Robótica de la
Universidad Rey Juan Carlos. La idea original surge como resultado de una tesis doctoral en el año 2003.
JdeRobot abstrae al programador del acceso a los dispositivos hardware, como la lectura de los sensores
o el envío de comandos a los motores. Principalmente está desarrollado en C y C++, aunque existen
componentes desarrollados en otros lenguajes como Java o Python. Ofrece un entorno de programación
basado en componentes donde la aplicación está formada por varios componentes y cada uno se ejecuta como
un proceso. Es distribuido y multilenguaje, los componentes interoperan entre sí a través del middleware de
comunicaciones ICE. Podemos encontrar componentes ejecutándose en distintas máquinas interoperando a
través de interfaces ICE, incluso estando escritos en lenguajes diferentes. Típicamente se crear aplicaciones
muy complejas a partir de componentes más simples que han sido desarrollados para realizar una tarea
especifica.
JdeRobot ofrece una interfaz sencilla para la programación de sistemas de tiempo real y resuelve
problemas relacionados con la sincronización de los procesos y la adquisición de datos. JdeRobot simplifica
el acceso a los dispositivos hardware desde el componente, permitiendo obtener la lectura de un sensor
a través de una interfaz ICE. Normalmente en las aplicaciones robóticas desarrolladas con JdeRobot, los
componentes necesitan datos sensoriales y envían ordenes a los actuadores. Los datos sensoriales llegan al
componente a través de interfaces ICE. Los drivers encargados de controlar los robots también reciben las
órdenes a través de interfaces ICE.
1 http://jderobot.org/
4
Figura 1: Ejemplo de componentes JdeRobot
La plataforma JdeRobot actualmente soporta un gran variedad de dispositivos como el robot Pioneer
de MobileRobotics Inc., el humanoide NAO de Aldebaran Robotics, el robot Kobuki de Yujin Robot, el
cuadricóptero AR.Drone de Parrot, cámaras firewire, USB e IP, los escáneres laser LMS de SICK y URG
de Hokuyo, los simuladores Stage y Gazebo, sensores de profundidad como kinect y otros dispositivos X10
de dómotica. Además, tiene soporte para software externo como OpenGL, GTK, XForms, Player, GSL
y OpenCV. Es un proyecto de software libre licenciado como GPLv32 , actualmente se encuentra en la
versión 5.2.
JdeRobot utiliza ICE para las comunicaciones entre sus nodos, por tanto, la tarea de leer valores
de un sensor o comandar órdenes a un robot es tan sencilla como ejecutar un método de un objeto de
la aplicación. Una ventaja significativa es la posibilidad de poder desarrollar aplicaciones independientes
del contexto. Un programador puede desarrollar un driver en C++ para un determinado robot que se
encuentre embebido en el propio robot, por otro lado, otro programador puede desarrollar una aplicación
2 http://www.gnu.org/licenses/gpl-3.0-standalone.html
5
para el procesamiento de imágenes en Python que se ejecute en un PC. Gracias a ICE podemos conectar
estos dos componentes, que en un principio eran independientes, como una única aplicación sin tener que
preocuparnos por las comunicaciones a bajo nivel. Con esto conseguimos desarrollar aplicaciones modulares
de una mayor complejidad sin esfuerzo adicional.
1.1.
Middleware de comunicaciones: ICE
ICE (Internet Communications Engine), es un middleware orientado a objetos que provee llamadas a
procedimientos remotos, computación grid y funcionalidad cliente/servidor desarrollada por ZeroC3 bajo
una doble licencia GNU GPL y una licencia propietaria. Está disponible para C++, Java, lenguajes .Net,
Objective-C, Python, PHP y Ruby, en la mayoría de los sistemas operativos. También existe una variante
para teléfonos móviles denominada Ice-e.
ICE nos permite desarrollar aplicaciones distribuidas con el mínimo esfuerzo, abstrayendo al
programador de interactuar con las interfaces de red a bajo nivel. El proceso de desarrollo de la aplicación
sólamente debe centrarse en la lógica de ésta y no en las peculiaridades de la red. Es un middleware
multiplataforma y multilenguage, de este modo, podemos implementar clientes y servidores en distintos
lenguajes de programación y en distintas plataformas. ICE trabaja con objetos distribuidos, de tal manera
que dos objetos dentro de nuestra aplicación no tienen porque estar ejecutándose en la misma máquina.
Los objetos pueden estar en distintas máquinas y comunicarse a través de red gracias al envío de mensajes
entre ambos.
1.2.
Simulador Gazebo
Los simuladores en robótica son usados para crear mundos virtuales y observar cómo un robot simulado
actúa en dicho entorno. De esta forma pueden programarse aplicaciones robóticas y probarlas sin depender
de un robot físico, haciendo que las pruebas sean más baratas y menos peligrosas, puesto que si el robot
se choca o tiene un comportamiento extraño que no se había previsto es posible reiniciar la simulación sin
que el modelo real (o las personas cercanas) haya sufrido daños. Algunos de estos simuladores representan
los mundos en 3D y recrean la física de este (gravedad, colisiones...) permitendo visualizar el movimiento
del robot en escenarios muy realistas. Motores de física comunes son ODE (Open Dynamics Engine) y
Physx.
3 http://www.zeroc.com/
6
Figura 2: Humanoide de la competición VRC en Gazebo
Gazebo
4
es un simulador muy completo que se distribuye como software libre. Cuenta con modelos de
robots que pueden usarse directamente, además de incluir la posibilidad de que el usuario cree su propio
robot o entornos como pueden ser un campo o el interior de un edificio, incluyendo texturas, luces y
sombras. Simula la física de los cuerpos rígidos: choques, empujes, gravedad, etc. Dispone de una amplia
clase de sensores como cámaras, lásers, sensores de contacto, imu, etc. Los algoritmos pueden aplicarse a
los modelos cargando plugins, o librerías dinámicas. Muchos de estos modelos y plugins han sido creados
por la organización OSRF (Open Source Robotics Foundation), que desarrolla y distribuye software libre
para su uso en investigación robótica.
2.
Interfaces JdeRobot relevantes para el curso
En JdeRobot cada componente ofrece o se subscribe a una interface. Estas interfaces se definen en un
pseudolenguaje propietario de Zero-C y luego, haciendo uso de un traductor, se puede obtener el código
fuente correspondiente a la interface para distintos lenguajes de programación. Después de este paso es
tarea del programador definir el compotamiento de la interface. A continuación de muestran algunas de
las interfaces que utilizaremos durante el curso, que básicamente concretan el acceso a los sensores y
actuadores a bordo del cuadricóptero.
4 http://gazebosim.org/
7
2.1.
camera
Es la interface estándar de JdeRobot para el envío y recepción de imágenes entre componentes. Su
definición es la siguiente:
#i n c l u d e <image . i c e >
module j d e r o b o t {
// S t a t i c d e s c r i p t i o n o f a camera
c l a s s CameraDescription
{
s t r i n g name ;
string shortDescription ;
s t r i n g streamingUri ;
float fdistx ;
float fdisty ;
f l o a t u0 ;
f l o a t v0 ;
f l o a t skew ;
f l o a t posx ;
f l o a t posy ;
f l o a t posz ;
f l o a t foax ;
f l o a t foay ;
f l o a t foaz ;
float roll ;
};
//Camera i n t e r f a c e
i n t e r f a c e Camera e x t e n d s Im age Pro vi der
{
idempotent C a m e r a D e s c r i p t i o n g e t C a m e r a D e s c r i p t i o n ( ) ;
i n t setCameraDescription ( CameraDescription d e s c r i p t i o n ) ;
s t r i n g st a r t C a me r a S t r ea m i n g ( ) ;
v o i d stopCameraStreaming ( ) ;
void r e s e t ( ) ;
};
} ; /∗ module ∗/
Como se aprecia en la definición, la interface camera nos permite definir una cámara y la posiblidad de
implementar de alguno de sus métodos como getCameraDescription(). Esta interface depende de la
interface image la cual almacena la infomración de la imagen en sí como el ancho y alto de la imagen o la
secuencia de bytes que componen la imagen.
// S t a t i c d e s c r i p t i o n o f t h e image s o u r c e .
8
c l a s s ImageDescription
{
i n t width ; /∗ Image width [ p i x e l s ] ∗/
i n t h e i g h t ; / ∗ Image h e i g h t [ p i x e l s ] ∗/
i n t s i z e ; / ∗ Image s i z e [ b y t e s ] ∗/
s t r i n g format ; /∗ Image format s t r i n g ∗/
};
//A s i n g l e image s e r v e d a s a s e q u e n c e o f b y t e s
c l a s s ImageData
{
Time timeStamp ; /∗ TimeStamp o f Data ∗/
ImageDescription d e s c r i p t i o n ;
ByteSeq p i x e l D a t a ; /∗ The image data i t s e l f . ∗ /
};
/ / ! I n t e r f a c e t o t h e image consumer .
i n t e r f a c e ImageConsumer
{
/ / ! Transmits t h e data t o t h e consumer .
v o i d r e p o r t ( ImageData o b j ) ;
};
// I n t e r f a c e t o t h e image p r o v i d e r .
i n t e r f a c e Im age Pro vi der
{
// Returns t h e image s o u r c e d e s c r i p t i o n .
idempotent I m a g e D e s c r i p t i o n g e t I m a g e D e s c r i p t i o n ( ) ;
// Returns t h e l a t e s t data .
[ " amd " ] idempotent ImageData getImageData ( )
throws DataNotExistException , H a r d w a r e F a i l e d E x c e p t i o n ;
};
} ; // module
Durante las prácticas utilizaremos la interface camera para obtener imágenes desde nuestro AR.Drone
simulado o desde un vídeo.
2.2.
Pose3D
La interface Pose3D nos permite definir la posición de un objeto en el espacio 3D. Está compuesta por
un cuaternión, que define la orientación del objeto, y un punto 3D en coordenadas homogéneas que indica
la posición de éste en el espacio.
c l a s s Pose3DData
{
float x;
9
float y;
float z ;
float h;
f l o a t q0 ;
f l o a t q1 ;
f l o a t q2 ;
f l o a t q3 ;
};
// I n t e r f a c e t o t h e Pose3D .
i n t e r f a c e Pose3D
{
idempotent Pose3DData getPose3DData ( ) ;
i n t setPose3DData ( Pose3DData data ) ;
};
} ; // module
Esta interface la utilizaremos para conocer la posición 3D y orientación del drone en nuestro entorno
simulado. Típicamente esta información se recopila en el robot real del sensor GPS y de la unidad
inercial IMU. Hay muchas maneras de representar posición y orientación tridimensionales (ángulos de
Euler, cuaterniones, yaw-pitch-roll, matríces, etc.), se ha elegido esta que tiene ventajas computacionales
y simplifica el acceso a ella desde otros componentes.
2.3.
cmd_vel
Con la interface cmd_vel podremos enviar comandos de velocidad a los componentes que la
implementen, entre ellos el driver del cuadricóptero. Es una interface pensada para el manejo de robots
aéreos y con ella podemos realizar translaciones y rotaciones sobre los ejes X, Y, Z.
c l a s s CMDVelData
{
f l o a t linearX ;
f l o a t linearY ;
float linearZ ;
f l o a t angularX ;
f l o a t angularY ;
f l o a t angularZ ;
};
i n t e r f a c e CMDVel{
i n t setCMDVelData ( CMDVelData data ) ;
};
} ; \ \ module
10
Durante las prácticas la utilizaremos para comandar órdenes de movimiento al AR.Drone. Cómo
estas órdenes se materializan en comandos a cada uno de los motores del cuadricóptero es un detalle
de implementación del que se encarga el driver del cuadricóptero, que ya tiene en cuenta la dinámica de
las hélices y del cuerpo para traducir esas órdenes de alto nivel a comandos concretos a cada motor.
2.4.
ardrone_extra
Esta interface es única para el manejo de AR.Drone, nos permite realizar las maniobras básicas de un
drone como el aterrizaje y el despegue. Además ofrece otras características únicas para AR.Drone como la
posibilidad de grabar vídeos en una memoría USB o realizar animaciones leds.
i n t e r f a c e ArDroneExtra {
v o i d toggleCam ( ) ;
void land ( ) ;
void t a k e o f f ( ) ;
void r e s e t ( ) ;
v o i d recordOnUsb ( b o o l r e c o r d ) ;
v o i d ledAnimation ( i n t type , f l o a t d u r a t i o n , f l o a t r e q ) ;
v o i d f l i g h t A n i m a t i o n ( i n t type , f l o a t d u r a t i o n ) ;
void flatTrim ( ) ;
};
};
2.5.
motors
La interface motors nos permite comandar órdenes de movimiento a robots con ruedas. La utilizaremos
para controlar el robot turtlebot que utilizaremos en la última práctica.
// I n t e r f a c e t o t h e Gazebo Motors A c t u a t o r s i n t e r a c t i o n .
i n t e r f a c e Motors
{
f l o a t getV ( ) ;
i n t setV ( f l o a t v ) ;
f l o a t getW ( ) ;
i n t setW ( f l o a t w ) ;
f l o a t getL ( ) ;
i n t setL ( f l o a t l ) ;
};
} ; // module
11
3.
Componentes relevantes para el curso
En esta sección veremos una introducción a los componentes JdeRobot que utilizaremos durante las
prácticas.
3.1.
Servidor cameraserver
Este componente implementa la interface camera descrita anteriormente. La Figura 3 muestra el
esquema de este componente. Como se aprecia en la imagen, cameraserver oferta la interface camera
a otros componentes clientes. Las imágenes que envía las obtiene desde una fuente de vídeo, la cual puede
ser una webcam o un fichero de vídeo.
Figura 3: Esquema de cameraserver
Todos los componentes de JdeRobot disponen de un fichero de configuración que se carga al inicio
de la ejecución. Si el componente ofrece alguna interface, estos ficheros suelen contener parámetros que
modifican el comportamiento del componente y una cadena de conexión donde se ofertará la interface. A
continuación se muestra el fichero de configuración de cameraserver.
CameraSrv . DefaultMode=1
CameraSrv . TopicManager=IceStorm / TopicManager : d e f a u l t −t 5000 −p 10000
#G e n e r a l C o n f i g
CameraSrv . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9999
CameraSrv . NCameras=1
CameraSrv . Camera . 0 . Name=cameraA
#0 c o r r e s p o n d s t o / dev / vid eo0 , 1 t o / dev / vide o1 , and s o on . . .
CameraSrv . Camera . 0 . Uri=0
CameraSrv . Camera . 0 . FramerateN=15
CameraSrv . Camera . 0 . FramerateD=1
CameraSrv . Camera . 0 . Format=RGB8
CameraSrv . Camera . 0 . ImageWidth=320
CameraSrv . Camera . 0 . ImageHeight =240
12
# I f you want a m i r r o r image , s e t t o 1
CameraSrv . Camera . 0 . M i r r o r=1
Si el componente se subscribe a una interface, el fichero de configuración contendrá la cadena de conexión
del componente que oferta la interface. El siguiente fichero de configuración pertenece al componente
cameraview que es un visor de imágenes.
Cameraview . Camera . Proxy=cameraA : d e f a u l t −h l o c a l h o s t −p 9999
3.2.
Servidor ardrone_server
El componente ardrone_server es el encargado de la comunicación con el robot AR.Drone ofreciendo
acceso a los sensores y rotores del cuadricóptero. Se trata de un envoltorio (wrapper) en C++ que encapsula
el SDK oficial de Parrot para el manejo de AR.Drone.
Figura 4: Esquema de ardrone_server
Para el manejo del AR.Drone en un entorno simulado existe un componente JdeRobot equivalente en
forma de plugin para Gazebo. El fichero de configuraicón de este componente es el siguiente:
ArDrone . Camera . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9999
ArDrone . Camera . Name=ardrone_camera
ArDrone . Camera . FramerateN=15
ArDrone . Camera . FramerateD=1
ArDrone . Camera . Format=RGB8
ArDrone . Camera . ArDrone2 . ImageWidth=640
ArDrone . Camera . ArDrone2 . ImageHeight =360
ArDrone . Camera . ArDrone1 . ImageWidth=320
ArDrone . Camera . ArDrone1 . ImageHeight =240
ArDrone . Camera . M i r r o r=0
ArDrone . Pose3D . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9998
ArDrone . Pose3D . Name=ardrone_pose3d
13
ArDrone . RemoteConfig . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9997
ArDrone . RemoteConfig . Name=ardrone_remoteConfig
ArDrone . Navdata . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9996
ArDrone . Navdata . Name=ardrone_navdata
ArDrone . CMDVel . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9995
ArDrone . CMDVel . Name=ardrone_cmdvel
ArDrone . Extra . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9994
ArDrone . Extra . Name=a r d r o n e _ e x t r a
3.3.
Componente introrob_py
Este componente nos permite teleoperar el AR.Drone y obtener sus datos sensoriales, tanto para el
cuadricóptero real como para el simulado.
Figura 5: Esquema de introrob_py con AR.Drone real
Como muestran las Figuras 5 y 6 introrob_py es un componente JdeRobot que se suscribe a las
interfaces ofertadas tanto por ardrone_server como por el plugin equivalente en Gazebo.
Figura 6: Esquema de introrob_py con AR.Drone simulado
14
Además de ésta interconexión introrob_py ofrece al usuario una interface gráfica para el manejo del
AR.Drone (ver Figura 7a) y la visualización de los datos sensoriales (ver Figura 7b).
(a) Teleoperación del drone
(b) Visualización de las imágenes
Figura 7: Interface gráfica de introrob_py
La Figura 9 muestra (de izquierda a derecha) el indicador de actitud (pitch y roll), la orientación del
drone (yaw), un altímetro, un indicador del porcentaje de carga de la batería y tres velocímetros que
indican la velocidad lineal en los ejes X, Y, Z.
Figura 8: Esquema de introrob_qt con el robot turtlebot
El código de tu práctica se empotrará dentro de este componente, y es tu código el que tomará las
decisiones de movimiento adecuadas en función de la información sensorial. En esto reside la “inteligencia”
del cuadricóptero y que su comportamiento sea el adecuado o no.
3.4.
Componente introrob_qt
Este componente lo usaremos para teleoperar el movimiento de un robot en tierra, el Kobuki, que se
moverá por el suelo mientras el cuadricóptero lo persigue.
15
Figura 9: Visualización de los datos sensoriales
De modo análogo al componente introrob_py este componente dispone una interface gráfica para el
manejo del robot.
Figura 10: Interface gráfica de introrob_qt
A diferencia del anterior, este componente ha sido desarrollado para el manejo de robots terrestes como
es el caso de Kobuki.
Figura 11: Visualización de la cámaras del robot turtlebot
16
4.
Práctica 1: Percepción visual a bordo del drone con cámara
En esta práctica se pretende desarrollar un filtro de color que nos permita segmentar algún objeto que se
encuentre en la imagen. Para un correcto seguimiento es necesario que el alumno cuente con los siguientes
conocimientos; espacios de color RGB y HSV, conocimientos sobre en entorno de desarrollo JdeRobot5 ,
conocimientos de programación (especialmente con el lenguaje Python) y conocimientos básicos sobre la
librería de visión artificial OpenCV6 .
La práctica se realizará con la versión de OpenCV 2.4.37 , la rama de Python 2.7 sobre JdeRobot 5.
4.1.
Configuración del entorno
Para la realización de la práctica se entrega un esqueleto de una aplicación (colorFilter) Python
que permitirá la recolección y visualización de las imágenes. Las imágenes se obtienen de dos vídeos
desarrollados para la práctica a través del componente de JdeRobot cameraserver.
4.1.1.
Componente cameraserver
El componente cameraserver oferta a tavés de sus interfaces camera.ice8 y image.ice9 las imágenes
obtenidas desde un vídeo o una cámara. Para esta práctica ofrecerás cameraserver ofertará las imágenes
que obtenga de los vídeos pelota_roja.avi y pelotas_roja_azul.avi.
Para que cameraserver comience a servir las imágenes requiere que antes se especifiquen algunos
parámetros de configuración en el fichero cameraserver.cfg. El fichero tiene el siguiente formato:
CameraSrv . DefaultMode=1
CameraSrv . TopicManager=IceStorm / TopicManager : d e f a u l t −t 5000 −p 10000
#G e n e r a l C o n f i g
CameraSrv . Endpoints=d e f a u l t −h 0 . 0 . 0 . 0 −p 9999
CameraSrv . NCameras=1
CameraSrv . Camera . 0 . Name=cameraA
#0 c o r r e s p o n d s t o / dev / vid eo0 , 1 t o / dev / vide o1 , and s o on . . .
#CameraSrv . Camera . 0 . Uri=0
CameraSrv . Camera . 0 . Uri=/home/NOMBRE DE USUARIO/ Videos / p e l o t a _ r o j a . a v i
CameraSrv . Camera . 0 . FramerateN=15
CameraSrv . Camera . 0 . FramerateD=1
CameraSrv . Camera . 0 . Format=RGB8
CameraSrv . Camera . 0 . ImageWidth=320
5 http://jderobot.org/Main_Page
6 http://opencv.org/
7 http://docs.opencv.org/2.4.3/modules/refman.html
8 http://svn.jderobot.org/jderobot/trunk/src/stable/interfaces/slice/jderobot/camera.ice
9 http://svn.jderobot.org/jderobot/trunk/src/stable/interfaces/slice/jderobot/image.ice
17
CameraSrv . Camera . 0 . ImageHeight =240
# I f you want a m i r r o r image , s e t t o 1
CameraSrv . Camera . 0 . M i r r o r=1
En concreto nos interesan los parámetros CameraSrv.Endpoints y CameraSrv.Camera.0.Uri. El primero
de ellos indica la dirección y puerto donde cameraserver servirá las imágenes, el segundo indica la dirección
del vídeo que servirá. Además de un fichero también se puede indicar la URI de una webcam, aunque para
la práctica no es necesario.
Con los parámetros configurado el siguiente paso será la ejecución de cameraserver desde la línea de
comandos, para ello es necesario ejecutar el siguiente comando:
robotica@ubuntu : ~ / c o l o r F i l t e r $ c a m e r a s e r v e r −−I c e . C o n f i g=c a m e r a s e r v e r . c f g
Una vez ejecutado el comando cameraserver comenzará a servir imágenes en la dirección Ip local por el
puerto 9999.
4.1.2.
Componente colorFilter
colorFilter es la aplicación desarrollada para la realización de la práctica que nos permitirá la recepción,
manipulación y visualización de las imágenes. Esta escrita en Python y se estructura del siguiente modo:
main.py es la función principal de la aplicación donde se inicializan los componentes.
sensors/ este directorio contiene el codigo fuente de las clases encargadas de la recolección y
manipulación de las imágenes.
gui/ todo el código encargado de la interfaz de usuario se encuentra en este directorio.
El esqueleto de la aplicación se entrega junto con la práctica y se espera que el alumno añada el código
necesario para la realización de la práctica. Concretamente será necesario modificar el método execute()
del fichero MyAlgorithm.py.
Se puede ejecutar colorFilter desde el IDE de desarrollo o desde la consola ejecutándo el siguiente
comando:
robotica@ubuntu : ~ / c o l o r F i l t e r $ p y t h o n main . py −−I c e . C o n f i g=i n t r o r o b _ p y . c f g
Donde el fichero introrob_py.cfg tiene el siguiente contenido:
I n t r o r o b . Camera . Proxy = cameraA : d e f a u l t −h l o c a l h o s t −p 9999
#I n t r o r o b . Pose3D . Proxy = ImuPlugin : d e f a u l t −h l o c a l h o s t −p 9000
#I n t r o r o b . CMDVel . Proxy = CMDVel : d e f a u l t −h l o c a l h o s t −p 9850
#I n t r o r o b . Navdata . Proxy = Navdata : d e f a u l t −h l o c a l h o s t −p 9700
#I n t r o r o b . Extra . Proxy = Extra : d e f a u l t −h l o c a l h o s t −p 9701
18
Cuando se ejecute la aplicación ésta se conectará al componente cameraserver para recibir las imágenes.
La Figura 12 muestra la ejecución de la aplicación.
Figura 12: Aplicación colorFilter
En la imagen de la izquierda de la Figura 12 se puede apreciar la imagen obtenida desde cameraserver.
La imagen de la derecha se corresponde con el resultado del proceso de segmentación que implementaremos
durante la práctica. La aplicación cuenta con 6 sliders que nos permitirán ajustar los valores de nuestro
filtro de color, además cuenta con un botón para reestablecer los valores del filtro y un checkbox para
mostrar la imagen final con la detección de la pelota en la imagen.
4.2.
Programando la detección de un objeto
Para la superación de la práctica el alumno tendrá que implementar un filtro de color que permita
segmentar la pelota y detectar su posición en la imagen. Así, dada una imagen de entrada como la de
la Figura 13a, el alumno tendrá que implementar el método thresoldImage() de la clase sensor para
obtener una imagen como la de la Figura 13b.
19
(a) Imagen de entrada
(b) Resultado de la segmentación
Figura 13: Segmentación de la imagen de entrada
Con la pelota segmentada (ver Figura 13b) el alumno tendrá que detectar la pelota en la imagen (ver
Figura 14) implementando para ello el método detectObject() de la clase sensor.
Figura 14: Detección de la pelota en la imagen
Un posible pipeline para el sistema de percepción visual sería el que muestra la Figura 15.
Figura 15: Pipeline del sistema de percepción
Los pasos a seguir para la implementación del filtro son:
1. Abre con tu entorno de programación la clase sensor que se encuentra en el fichero
colorFilter/sensors/sensor.py.
2. Dirigite a la definición del método thresoldImage().
3. Dentro del método el primer paso será obtener la imagen que queremos segmentar, para obtener las
imágenes enviadas por cameraserver la clase sensor contiene el método getImage(). Éste método
devuelve la imagen de entrada (no te olvides almacenarlo en una variable).
20
4. Debido al ruido o a las imperfecciones que pueda contener la imagen es recomendable aplicar algún
filtro de suavizado. Para la práctica se recomienda utilizar un filtro gaussiano, en OpenCV puedes
encontrarlo como GaussianBlur10 .
5. Las imágenes que obtenemos de cameraserver se encuentran en el espacio de color RGB. Aunque
es un buen espacio para la representación de imágenes digitales es muy delicado frente a cambios de
luz en el ambiente. Por ello, el siguiente paso será convertir nuestra imagen al espacio de color HSV.
Se recomiendo utilizar la función cvtColor()11 de OpenCV.
6. Llegados a este punto estamos en disposición de aplicar nuestro filtro de color. Los sliders que vimos
en la Figura 12 almacenan su valor en una serie de atributos de la clase sensor. En total tenemos
6 variables que almacenan estos valores: hmin, hmax, vmin, vmax, smin y smax. Como nuestro
método thresoldImage() pertenece a la clase sensor podemos acceder al valor de estos atributos
directamente. Para aplicar el filtro puedes utilizar la función inRange()12 de OpenCV. Esta función
debe recibir 6 parámetros porque nuestras imágenes tienen 3 canales (HSV).
7. Por último, para poder visualizar la imagen en la aplicación es necesario establecer la imagen
resultante del filtro en el atributo de la variable imgThresold de la clase sensor. Para ello invoca al
método setThresoldImage() indicando como parámetro la imagen obtenida en el filtrado.
Con los píxeles obtenidos el siguiente paso es la detección de la pelota en la imagen. Los posibles pasos
serían:
1. Abre con tu entorno de programación la clase sensor que se encuentra en el fichero
colorFilter/sensors/sensor.py.
2. Dirigite a la definición del método detectObject().
3. Encontrar un objeto blanco sobre un fondo negro es mucho más fácil que intentar encontrar una
pelota roja sobre un fondo cambiante. Por ello en primer lugar tenemos que obtener la imagen binaria
obtenida en el proceso de segmentación. Para ello puedes utilizar el método getThresoldImage() de
la clase sensor. En lugar de utilizar directamente esta imagen es recomendable que hagas una copia
de ella, puedes invocar a la función copy()13 de numpy.
4. Una buena aproximación para detectar la pelota podría ser la detección del contorno del objeto. En
OpenCV existe la función findContours()14 .
5. La función anterior nos devolverá la lista de puntos que conforman el contorno del objeto. Estos
puntos se pueden aproximar a polígonos que después podremos encerrar en un rectángulo tal y
10 http://docs.opencv.org/2.4.3/modules/imgproc/doc/filtering.html?highlight=gaussianblur#cv2.GaussianBlur
11 http://docs.opencv.org/2.4.3/modules/imgproc/doc/miscellaneous_transformations.html?highlight=cvtcolor#
cv2.cvtColor
12 http://docs.opencv.org/2.4.3/modules/core/doc/operations_on_arrays.html?highlight=inrange#cv2.inRange
13 http://docs.scipy.org/doc/numpy/reference/generated/numpy.copy.html
14 http://docs.opencv.org/2.4.3/modules/imgproc/doc/structural_analysis_and_shape_descriptors.html?
highlight=findcontours#cv2.findContours
21
como muestra la Figura 14. Puedes utilizar las funciones approxPolyDP()15 , boundingRect()
17
rectangle()
16
y
de OpenCV.
6. Al trabajar en entornos con ruido es probable que la aproximación de polígonos a rectángulos que
hemos visto en el punto anterior nos devuelva más de un rectángulo cuando en realidad solamente
tenemos un objeto. En este caso es recomendable filtrar los rectángulos obtenidos para quedarnos
con alguno que cumpla ciertas características, como por ejemplo el tamaño del rectángulo.
7. Después del filtrado de rectángulos deberíamos tener un único rectángulo donde se encuentre nuestro
objeto. Gracias a sus coordenadas, su altura y el ancho de éste podremos calcular el centro del objeto
que hemos detectado.
8. Por último, para poder visualizar el resultado en la ventana de detección es necesario establecer la
imagen resultante en la variable detectionImage de la clase sensor. Para ello invoca al método
setDetectionImage() indicando como parámetro la imagen obtenida en el proceso de detección.
highlight=approxpolydp#cv2.approxPolyDP
highlight=boundingrect#cv2.boundingRect
17 http://docs.opencv.org/2.4.3/modules/core/doc/drawing_functions.html?highlight=rectangle#cv2.rectangle
22
5.
Práctica 2: Navegación local por posición
En esta práctica vamos a aprender el uso de los controladores PID para implementar un algoritmo de
navegación local en el cuadricóptero. Para ello, junto con el enunciado, se proporciona el código fuente del
componente controlPID donde el alumno tendrá que implementar el algoritmo.
5.1.
En este apartado se detalla la configuración del entorno necesaria para la elaboración de la práctica.
5.1.1.
Gazebo
Para esta práctica se ha diseñado un mundo para el simulador Gazebo. Este mundo dispone de un
modelo 3D del cuadricóptero AR.Drone y 5 balizas dispuestas en modo de cruz, tal y como muestra la
Figura 16.
Figura 16: Mundo de Gazebo para la práctica
Para ejecutar Gazebo con este mundo realiza los siguientes pasos:
cd ~ / . gazebo / c f g / c o n t r o l P I D /
gazebo beacons−a r d r o n e . world
Con el comando anterior Gazebo habrá lanzado el mundo beacons-ardrone.world con el plugin de AR.Drone
para Gazebo. De tal modo que el plugin está ofertando las siguientes interfaces:
camera, en el puerto 9995
navdata, en el puerto 9700
23
cmd_vel, en el puerto 9580
ardrone_extra, en el puerto 9701
Pose3D, en el puerto 9000
En el componente controlPID entregado con la práctica, las interfaces ya están configuradas (en la clase
sensor.py) por lo que no es necesario modificarlas.
5.1.2.
Componente controlPID
El componente controlPID en realidad es el componente introrob_py que ya vimos en la sección 3.3.
La diferencia de este nuevo componente es que contiene una nueva clase que se llama Beacon.py. Esta
clase contiene:
Un objeto de la clase Pose3D con las coordenadas X,Y,Z de la baliza en el mundo de Gazebo y un
cuarternión con todos sus valores a 0.
Una variable booleana, active, que indica que la baliza está activa. Durante la navegación con
AR.Drone sólo una baliza puede estar activa, ésta será la baliza actual a la que el cuadricóptero se
dirige.
Una variable booleana, reached, que indica si la baliza ha sido alcanzada.
De este modo si quisieramos obtener desde la clase MyAlgorithm.py las coordenadas 3D de la primera
baliza podríamos emplear la siguiente secuencia de instrucciones.
c oo r de n a da X Ba l iz a 1 = s e l f . b e a c o n s [ 0 ] . g e t P o s e ( ) . x
c oo r de n a da Y Ba l iz a 1 = s e l f . b e a c o n s [ 0 ] . g e t P o s e ( ) . y
Si quisieramos indicar que la primera baliza está activa y no ha sido alcanzada podríamos ejecutar lo
siguiente.
s e l f . b e a c o n s [ 0 ] . s e t A c t i v e ( True )
s e l f . b e a c o n s [ 0 ] . setRe ached ( F a l s e )
Iterando sobre la lista beacons podríamos obtener la información de todas las balizas del mundo.
f o r beacon i n s e l f . b e a c o n s
beacon . g e t P o s e ( ) . x
beacon . s e t A c t i v e ( F a l s e )
La Figura 17 muestra los sistemas de referencia 3D del mundo de Gazebo y del cuadicóptero.
24
Figura 17: Sistemas de referencia 3D
La clase sensor.py nos proporciona algunos métodos para interactuar con AR.Drone. Para esta
práctica podrían ser de interés los siguientes métodos:
setVX() y setVY(), estos métodos reciben como parámetro un valor en coma flotante (float) entre
-1 y 1. Con setVX() se establece la velocidad lineal en el eje X de AR.Drone, su frente: hacia delante
si el valor es positivio y hacia atrás si el valor es negativo. Con setVY() se establece la velocidad
lineal en el eje Y, su lateral: hacia la izquierda si el valor es positivo y hacia la derecha si el valor es
negativo. Después de ejecutar éstos métodos es necesario ejecutar el método sendVelocities() sin
ningún parámetro para comandar la órden.
sendCMDVel(), este método recibe 6 parámetros: vy, vx, vz, yaw, roll y pitch. Cada uno de los valores
se debe indicar entre -1 y 1. Los valores roll y pitch no tienen efecto en el mundo simulado de Gazebo.
Al contrario que los métodos anteriores, este método envía directamente la órden al cuadricóptero.
Podemos comandar órdenes de movimiento desde la clase MyAlgorithm.py del siguiente modo.
s e l f . s e n s o r . setVX ( 0 . 5 )
s e l f . sensor . sendVelocities ()
El código anterior comandará al drone la orden de moverse hacia delante a una velocidad de 0.5 (a la mitad
de potencia). Esta órden estará activa hasta que se le indique lo contrario, de modo que si quisieramos que
el drone parase tendríamos que hacerlo del siguiente modo.
25
s e l f . s e n s o r . setVX ( 0 )
s e l f . sensor . sendVelocities ()
Para conseguir el mismo resultado que los comandos anteriores, podríamos utilizar el método
sendVelocities() con 6 valores.
s e l f . s e n s o r . sendCMDVel ( 0 , 0 . 5 , 0 , 0 , 0 , 0 )
El código anterior comanda la órden de moverse hacia delante de manera inmediata. Este método nos
permite comandar distintas órdenes a la vez.
s e l f . s e n s o r . sendCMDVel ( − 0 . 4 , 0 . 5 , 0 . 2 , 0 . 1 , 0 , 0 )
El comando anterior provocará que el drone se mueva hacia delante a una velocidad de 0.5, se translade
hacia la derecha a 0.4, se eleve en el eje Z a 0.2 y rote sobre el eje Z a una velocidad de 0.1. Finalmente,
para detener el movimiento del drone se puede utilizar la siguiente instrucción.
s e l f . s e n s o r . sendCMDVel ( 0 , 0 , 0 , 0 , 0 , 0 )
Además de poder enviar órdenes de movimiento a AR.Drone también podremos obtener su posición 3D.
s e l f . s e n s o r . getPose3D ( ) . x
s e l f . s e n s o r . getPose3D ( ) . y
s e l f . s e n s o r . getPose3D ( ) . z
Las instrucciones anteriores nos devolverán las coordenadas X,Y y Z del drone dentro del mundo de Gazebo.
5.2.
Implementando un algoritmo de navegación local
Para la superación de la práctica el alumno tendrá que implementar un algoritmo de navegación local
basado en controladores PID. Concretamente se utilizarán dos controladores PID, uno para el control del
drone sobre el eje X (ver Figura 17) y otro para el eje Y. El objetivo es alcanzar las 5 balizas (ver Figura
16) en orden. El resultado esperado se puede ver en la página web del curso18 .
Para implementar el algoritmo se recomienda seguir los siguientes pasos:
1. Abre con tu entorno de programación el componente controlPID y dirígete a la clase MyAlgorithm
que se encuentra en el fichero controlPID/MyAlgorithm.py.
2. Dirígete a la definición del método execute().
3. En el método anterior tendrás que implementar tu algoritmo. Para empezar puedes crear una nueva
clase con el nombre PID donde puedes introducir el código del controlador.
4. Con la clase PID implementada, puedes instanciar dos objetos de dicha clase en el constructor de la
clase MyAlgorithm.
18 http://jderobot.org/Programacion-de-drones#Pr.C3.A1ctica_sobre_los_controladores_PID
26
5. En el método execute() puedes recorrer en un bucle for todas las balizas del mundo. Tienes un
ejemplo en la sección 5.1.2.
6. Por cada baliza calcula el error de posición para los ejes X e Y. Para el eje X el error de posición será
el valor de la coordenada X de la baliza, menos el valor de la coordenada X del drone. De la misma
manera para el eje Y. Puedes ver cómo obtener las coordenadas 3D de una baliza y del AR.Drone
en la sección 5.1.2.
7. Indica a tus controladores PID que actualicen su valor con los errores que acabas de calcular.
8. Cada controlador PID tendrá que devolverte el valor que represente la velocidad que tendrás que
comandar al AR.Drone en cada eje. Estos valores los tendrás que utilizar con el método sendCMDVel()
(ver sección 5.1.2).
9. Cuando el error (distancia del drone a una baliza) sea inferior a un valor que hayas establecido (por
ejemplo a 0.1 metros), podrás marcar esa baliza como alcanzada y no activa (ver sección 5.1.2).
10. Repite los pasos del 6 al 9 para las 5 balizas. Recuerda que para poder ejecutar tu algoritmo debes
pulsar el botón Play.
11. Cuando hayas alcanzado todas las balizas, puedes pulsar el botón Stop del componente para dejar
de ejecutar tu algoritmo.
27
6.
Práctica 3: Seguimiento de objetos con AR.Drone
En esta última práctica obligatoria del curso haremos uso de los conceptos aprendidos en las anteriores
prácticas. Al igual que en las anteriores, con el enunciado de la práctica se adjunta el código fuente del
componente catchTheTurtle donde el alumno tendrá que implementar su algoritmo.
6.1.
6.1.1.
Gazebo
Para esta práctica se ha diseñado un mundo para el simulador Gazebo. En este nuevo mundo tendremos
dos robots: el cuadricóptero AR.Drone y el robot Kobuki, tal y como muestra la Figura 16.
Figura 18: Mundo de Gazebo para la práctica
Para ejecutar Gazebo con este mundo realiza los siguientes pasos:
cd ~ / . gazebo / c f g / c a t c h T u r t l e /
gazebo t u r t l e b o t −a r d r o n e . world
Con el comando anterior Gazebo habrá lanzado el mundo turtle-ardrone.world con los plugins de AR.Drone
y TurtleBot (también llamado Kobuki) para Gazebo. Como en la práctica anterior, el plugin de AR.Drone
oferta las siguientes interfaces:
camera, en el puerto 9995
navdata, en el puerto 9700
cmd_vel, en el puerto 9580
ardrone_extra, en el puerto 9701
28
Pose3D, en el puerto 9000
Por otro lado, el plugin de TurtleBot oferta las siguientes interfaces:
motors, en el puerto 8999
camera, un par estereo de cámaras en los puertos 8995 y 8994
encoders, en el puerto 8997
laser, en el puerto 8996
Pose3Dencoders, en los puertos 9992 y 9993
Pose3Dmotors, en los puertos 9990 y 9991
Para el plugin de AR.Drone, las interfaces ya están configuradas (en la clase sensor.py) por lo que no
es necesario modificarlas. Para el plugin de TurtleBot se adjunta (junto con el código del componente) el
fichero turtlebot.cfg.
La arquitectura software general de este escenario se muestra en la Figura 19.
Figura 19: Arquitectura software de práctica de persecución de robot terrestre
6.1.2.
Componente introrob_qt
Como ya vimos en la sección 3.4 el componente introrob_qt es un componente JdeRobot que nos
permitirá teleoperar el robot Kobuki. Una vez lanzado el mundo turtle-ardrone.world en Gazebo, podremos
teleoperar a Kobuki siguiendo los siguientes pasos:
cd c a t c h T h e T u r t l e /
i n t r o r o b _ q t −−I c e . C o n f i g=t u r t l e b o t . c f g
29
Utilizaremos este componente para mover al robot Kobuki por el mundo de Gazebo para que el AR.Drone
le siga gracias al algoritmo que implementaremos.
Figura 20: Robot Kobuki
6.1.3.
Componente catchTheTurtle
Como en la anterior práctica, el componente catchTheTurtle es una copia del componente
introrob_py que ya vimos en la sección 3.3.
Si en la sección 5.1.2 vimos cómo enviar comandos de velocidad a AR.Drone, en esta sección veremos
cómo obtener las imágenes de su cámara desde la clase MyAlgorithm.py.
droneImage = s e l f . s e n s o r . getImage ( )
La instrucción anterior nos devolverá la imagen de la cámara activa del drone y lo almacenará en la variable
droneImage. A partir de este momento, dicha variable contendrá una imagen que podremos tratar con las
técnicas que aprendimos en la primera práctica (ver sección 4).
6.1.4.
Para esta práctica necesitaremos detectar al robot Kobuki, el cual tiene colocado en su parte superior
una pegatina con dos rectángulos de distinto color (Figura 20). Aprovecando la arquitectura basada en
nodos de JdeRobot, podremos utilizar el componente colorFilter que desarrollamos en la práctica 1 para
30
obtener los valores de nuestro filtro HSV.
Para conectar el componente colorFilter al flujo de vídeo del plugin de AR.Drone, en primer lugar
será necesario ejecutar Gazebo tal y como se indica en la sección 6.1.1. En segundo paso será modificar
el código de colorFilter para cambiar la cadena de conexión. El código que tenemos que modificar se
encuentra en el constructor de la clase y es el siguiente.
basecamera = i c . propertyToProxy ( " Cameraview . Camera . Proxy " )
#basecamera = i c . s t r i n g T o P r o x y ( " cam_sensor_ventral : d e f a u l t −h l o c a l h o s t −p 9 9 9 4 " )
Lo tendremos que modificar tal y como se expone a continuación:
#basecamera = i c . propertyToProxy ( " Cameraview . Camera . Proxy " )
basecamera = i c . s t r i n g T o P r o x y ( " cam_sensor_ventral : d e f a u l t −h l o c a l h o s t −p 9 9 9 4 " )
Una vez que colorFilter se haya conectado al flujo de vídeo del drone, podremos modificar los valores
de nuestro filtro para detectar el rectángulo verde.
Figura 21: Componente colorFilter con las imágenes del AR.Drone
Los valores del filtro nos serán de utilidad en el desarrollo de la práctica para poder detectar al robot
Kobuki.
31
Figura 22: Detección del robot Kobuki con el componente colorFilter
6.2.
Desarrollando un algoritmo para el seguimiento de un robot terrestre
Para la superación de la práctica el alumno tendrá que implementar un algoritmo que permita al
cuadricóptero AR.Drone seguir al robot Kobuki. Para la realización del algoritmo se emplearán las técnicas
aprendidas en las prácticas 1 y 2 (ver secciones 4 y 5). Al igual que en la práctica 2 se utilizarán dos
controladores PID que permitiran gobernar el movimiendo del drone en sus ejes X e Y (ver Figura 17). El
objetivo es que el AR.Drone siga al robot Kobuki mientras éste es teleoperado a través del componente
introrob_qt. El resultado esperado se puede ver en la página web del curso19 .
Figura 23: Imagen obtenida desde la cámara ventral del AR.Drone
19 http://jderobot.org/Programacion-de-drones#Atrapa_a_la_tortuga
32
Para implementar el algoritmo se recomienda seguir los siguientes pasos:
1. Abre con tu entorno de programación el componente catchTheTurtle y dirígete a la clase
MyAlgorithm que se encuentra en el fichero catchTheTurtle/MyAlgorithm.py.
2. Dirígite a la definición del método execute().
3. En primer lugar se recomienda implementar un método que nos permita detectar en la imagen
obtenida del drone el rectángulo verde que utilizaremos para obtener la posición del robot Kobuki. En
este punto puedes seguir los pasos que realizamos en la práctica 1 (ver sección 4.2) para implementar
un filtro de color.
4. La detección del rectángulo verde nos devolverá las coordenadas X e Y del centro del rectángulo.
Podemos asumir que éstas coordenadas son el centro del robot Kobuki que queremos seguir.
5. Con el robot terrestre detectado, el siguiente paso será utilizar los controladores PID desarrollados en
la práctica 2 (ver sección 5.2). En este caso el error de posición será la distancia desde las coordenadas
X e Y del rectángulo detectado en la imagen hasta el centro de dicha imagen. Actualiza los errores
de distancia obtenidos en los controladores PID en cada iteración.
6. Los controladores PID nos devolverán el valor que tendremos que comandar al AR.Drone haciendo
uso del método sendCMDVel().
7. Como ya vimos en la práctica 2, tendremos que definir un error mínimo que nos indicará cómo de lejos
queremos que nuestro drone se encuentre de su objetivo. Por tanto, el objetivo de los controladores
PID será ahora minimizar el error en distancia hasta alcanzar el error mínimo, lo cual nos indicará
que el AR.Drone se encuentra encima del robot Kobuki.
8. Antes de ejecutar tu algoritmo pulsando sobre el botón Play, es necesario que teleoperes al AR.Drone
con el componente catchTheTurtle hasta posicionar al drone encima del robot Kobuki. El objetivo
es que la cámara ventral del drone pueda ver a Kobuki, tal y como muestra la Figura 23 y tenga una
referencia inicial. Una vez arrancado así, tu código gobernará al drone para que automáticamente
siga el movimiento del Kobuki mientras teleoperas a éste para que se desplace por el mundo.
9. Para terminar con la ejecución de tu algoritmo puedes pulsar el botón Stop.
33
7.
Práctica 4: Tu cuadricóptero sigue una carretera desde el aire
En esta práctica el objetivo es desarrollar un algoritmo de control visual que permita a nuestro
cuadricóptero desplazarse siguiendo una carretera desde el aire. Para ello utilizará las imágenes que se
obtienen de la cámara ventral y sus motores. Es una práctica opcional para aquellos alumnos que hayan
superado las prácticas anteriores y deseen profundizar más en la navegación por control visual.
Se adjunta un archivo con el mundo y los ficheros de configuración necesarios.
7.1.
7.1.1.
Gazebo
Se ha diseñado un mundo en Gazebo que contiene un cuadricóptero ArDrone y una carretera para que
la siga (Figura 24). Para ejecutar Gazebo con este mundo basta con copiar el mundo road_drone.world
en el directorio donde se encuentra el mundo de la práctica 3:
mv ~/ road_drone . world ~ / . gazebo / c f g / c a t c h T u r t l e
cd ~ / . gazebo / c f g / c a t c h T u r t l e
gazebo road_drone . world
De esta forma Gazebo arrancará con el mundo road_drone.world y la configuración de la práctica
anterior.
Figura 24: Mundo con la carretera que tu cuadricópteros ha de seguir
7.1.2.
Es conveniente disponer del componente colorFilter empleado en la práctica 1. Permitirá obtener
valores adecuados de los componentes HSV que nos permitan filtrar las imágenes de la cámara ventral del
cuadricóptero e identificar en ellas si cada píxel analizado es del color de la carretera o no.
34
Para utilizar este componente será necesaria la misma modificación en el código que realizamos en la
práctica 3. En caso de que el alumno haya terminado la tercera práctica y no haya realizado ningún cambio
en el código tras ello, no será necesario que realice este paso.
El puerto indicado en el fichero cameraview.cfg deberá ser el 9994. Una vez ejecutada, la aplicación se
conectará a la cámara ventral del AR.Drone para recibir las imágenes. El alumno utilizar el mismo código
que utilizó en la primera práctica para implementar el método thresoldImage y valerse de los diales para
obtener los valores adecuados para filtrar la carretera.
7.1.3.
Componente introrob_py
En esta práctica se usará el mismo introrob que en la anterior para tener la misma configuración. El
alumno tendrá que eliminar la implementación que diseñó para el método execute y reimplementarlo por
completo para lograr el objetivo de esta nueva práctica.
7.2.
Programando la detección de la carretera y el algoritmo de control
El comportamiento de seguir una carretera desde el aire se puede descomponer en una parte perceptiva
y una parte de control. Para la parte perceptiva el alumno tendrá que implementar un filtro de color,
se recomienda en HSV, que permita segmentar la carretera y detectar zonas de interés en determinadas
partes del fotograma. No es necesario que se analicen todos los píxeles de cada imagen, tal vez analizando
sólo unas cuantas líneas es más que suficiente para extraer de la imagen la información de donde está el
drone respecto del camino, desviado hacia un lado, hacia otro, mucho, poco, etc..
En cuanto al control, la diferencia con la práctica anterior consiste en que el objeto que queremos
detectar no se mueve. El objetivo ahora es que el drone se desplace sobre la carretera, desde el punto en el
que arranca hasta llegar al otro extremo. Igual que en la anterior práctica el alumno deberá implementar el
método execute en el fichero MyAlgorithm.py del componente introrob_py así como definir las clases y
métodos adicionales que crea oportunos. En la Figura 25 se muestra un ejemplo del cuadricóptero siguiendo
un camino desde el aire.
Puede ser útil definir ciertas filas o columnas en las imágenes para analizar píxel a píxel, comprobando
que se encuentran dentro del rango seleccionado. Para cada fila que se haya escogido analizar, seleccionar
las regiones de interés y escoger la más adecuada, dando la velocidad oportuna al cuadricóptero según la
posición de la región que el alumno haya escogido.
35
Figura 25: Cuadricóptero siguiendo una carretera, e imagen de la cámara ventral
36
8.
Práctica 5: Aterrizando encima de un coche
Programa tu cuadricóptero para que sea capaz de perseguir a un coche moviéndose en tierra y aterrizar
encima de él, incluso si se está moviendo. Como todos los comportamientos en robots, tiene una parte
perceptiva y una parte de actuación. Para simplificar este ejercicio hemos pintado encima del coche una
marca de colores fácil de identificar desde las imágenes en la cámara ventral del cuadricóptero. Además el
robot en tierra no se desplazará demasiado deprisa. Con este ejercicio se pretende que practiques con el
uso de autómatas para generar comportamiento en un drone, manteniendo control visual reactivo, y que
profundices en la detección robusta de objetos interesantes usando filtros de color en las imágenes.
8.1.
Configurando el entorno
En esta práctica el mundo simulado tiene tres actores: el escenario, un coche y un cuadricóptero, tal y
como muestra la figura 26. El escenario es estático y no hemos introducido muchos elementos para que el
simulador vaya fluido. Para ejecutar Gazebo con este mundo sigue los siguientes pasos:
cd ~ / . gazebo / c f g /
gazebo carColorBean . world
Figura 26: Coche que se mueve por el mundo simulado
Una vez arrancado el mundo hay que lanzar el componente introrob_qt y configurarlo para conectarse al
coche simulado. Desde el GUI desde este componente se puede teleoperar a voluntad al vehículo indicándole
más o menos velocidad y ángulos de giro, tal y como se muestra en la figura 27. Para ello sigue los siguientes
pasos:
$ v i s u a l L a n d e r / i n t r o r o b _ q t −−I c e . C o n f i g=c a r . c f g
37
Figura 27: Teleoperación del coche con una baliza en su techo
Finalmente tienes que programar la inteligencia de tu robot en un componente JdeRobot. Te
proporcionamos la plantilla del componente visualLander, que ya incluye algunas funcionalidades básicas
como obtener las imágenes de la cámara y la conexión con los motores del drone para comandarle órdenes
de movimiento. En este componente tendrás que insertar tu propio código en Python para materializar la
semántica que deseas en tu robot. También incluye un patrón para materializar autómatas de estado finito
en Python.
Para probar tu algoritmo tienes que lanzar el componente visualLander del siguiente modo:
$ v i s u a l L a n d e r / python main . py −−I c e . C o n f i g=i n t r o r o b _ p y _ s i m u l a t e d . c f g
Luego, con visualLander en ejecución, pulsa sobre el botón Take-off para despegar el drone y a
continuación sobre el botón Play para ejecutar tu algoritmo. La plantilla llamará a tu código unas 10 o
15 veces por segundo, será tu código el que procese las imágenes y decida qué órdenes se comandan a los
motores del drone en cada iteración.
8.2.
Programando un autómata de estado finito
Se recomienda programar la parte de control de este comportamiento en forma de autómata de estados
finito. En los autómatas el comportamiento se vertebra como estados y transiciones. En cada estado
el robot ejecuta cierta acción, o activa tal o cual controlador reactivo. A su vez, en cada estado vigila si se
presenta alguna situación que lo haga cambiar de estado (transiciones). El comportamiento se especifica, se
define, diseñando el conjunto de estados para cierto comportamiento, qué hacer en cada estado y cuáles son
las transiciones posibles. Las transiciones suelen ser condiciones comprobables en los valores sensoriales.
38
Figura 28: Posible autómata de estados
Típicamente con cuatro estados se cubre la funcionalidad pedida en esta práctica (Figura 28): búsqueda,
aproximación, descenso y parada. El robot arranca en el suelo, sin ver nada en su cámara y mucho menos
el coche a seguir. En la búsqueda el objetivo es que el drone localice al coche, mientras no lo haga seguirá en
este estado. La acción adecuada puede ser elevarse para abrir el campo visual o deambular por el espacio
3D para explorar, por ejemplo. Mientras lo hace busca en la imagen la marca visual, por si aparece el
coche.
Cuando se detecte la marca visual que hay encima del vehículo debe transitar al estado de aproximación.
En él se acercará a la vertical del vehículo en tierra, que puede estar estático o moverse ligeramente. En
este estado típicamente un control PID puede gobernar el movimiento del drone. La referencia es dónde se
observa la baliza en la imagen, su (X,Y), y la distancia al centro de la imagen. Unas decisiones de control
razonables son acelerar si el error es alto, moverse lentamente si es bajo o no moverse si apenas hay error
en ese eje.
Cuando el drone está centrado sobre el vehículo (es decir, cuando éste aparece aproximadamente en el
medio de la imagen ventral), entonces el robot puede pasar al siguiente estado: aterrizaje. En este último
estado se puede activar otro control PID, que además de mantener más o menos centrada la baliza visual en
la imagen hace descender al robot. ¿Hasta cuándo? Puedes utilizar la altura para decidir cuándo apagar y
no empujar hacia abajo con los motores, o puedes mantener al drone empujando hacia abajo, “pegándose”
al techo del coche.
Se suele añadir un estado final de parada donde se entra una vez que ha aterrizado, en el cual se ordena
reposo a los motores, y del que ya no se sale.
8.2.1.
Creando un autómata de estado finito en visualLander
El componente visualLander ofrece los mecanismos necesarios para la creación de los estados y el
autómata de estados finitos. Este mecanismo se divide en dos, en primer lugar en la clase Arbitrator:
class Arbitrator ( ) :
def __init__ ( s e l f , s t a t e L i s t , s t a r t S t a t e , s t o p S t a t e ) :
self . states = stateList
s e l f . activeState = startState
s e l f . lastState = stopState
def update ( s e l f ) :
i f s e l f . a c t i v e S t a t e i s not s e l f . l a s t S t a t e :
39
s e l f . states [ s e l f . activeState ] . action ()
s e l f . activeState = s e l f . states [ s e l f . activeState ] . transit ()
Tal y como muestra el trozo de código anterior, el constructor de esta clase recibe como parámetros
una lista de estados, el identificador del estado de arranque y el identificador del estado de parada. La
clase tiene el método update() que se ejecutará mientras el botón Play de visualLander esté activo. Este
método ejecuta, para el estado activo, el método action() y a continuación el método transit() ambos de la
clase State que más adelante veremos. La ejecución del método transit() debe devolver el identificador del
estado que será el activo para la siguiente iteración. Este comportamiento se ejecutará mientras el estado
activo no sea el estado de parada.
Los estados de nuestro autómata finito los modelaremos con la clase State:
import abc
from abc import ABCMeta
class State ( ) :
__metaclass__ = ABCMeta
@abc . a b s t r a c t m e t h o d
def a c t i o n ( s e l f ) :
pass
@abc . a b s t r a c t m e t h o d
def t r a n s i t ( s e l f ) :
pass
Esta clase es abstracta (no se pueden instanciar objectos de ella) por ello es necesario que cada estado se
defina en una clase que herede de State y rellene los métodos action() y transit(). Podemos ver un ejemplo
en el siguiente trozo de código:
c l a s s Busqueda ( S t a t e ) :
def __init__ ( s e l f , s e n s o r ) :
S t a t e . __init__ ( s e l f )
s e l f . sensor = sensor
s e l f . detected = f a l s e
def a c t i o n ( s e l f ) :
s e l f . detected = findBeacon ( )
i f s e l f . d e t e c t e d i s not True :
s e l f . s e n s o r . sendCMDVel ( 0 . 0 , 0 . 0 , 0 . 5 , 0 . 0 , 0 . 0 , 0 . 0 )
def t r a n s i t ( s e l f ) :
i f s e l f . d e t e c t e d i s True :
40
s e l f . detected = False
return S t a t e s .APROXIMACION
else :
return S t a t e s .BUSQUEDA
Cada identificador de estado de nuestro autómata está definido en la clase States. Este número de
identificación coincide con la posición en el que los estados fueron añadidos a la lista que se pasó al
constructor de la clase Arbritator. El siguiente trozo de código muestra los cuatro estados propuestos para
la práctica.
class States ( ) :
BUSQUEDA
= 0
APROXIMACION
= 1
DESCENSO
= 2
PARADA
= 3
De este modo, con nuestra clase Arbritator y una lista de estados podemos tener nuestro autómata de
estado finitos para la realización de la práctica.
8.3.
Programando la percepción de la baliza
La parte perceptiva se ha simplificado incorporando una baliza de colores encima del techo del coche
(Figura 26), de modo que con un sencillo filtro de color se pueda resolver. En autómatas la percepción
también se organiza por estados. Las cosas que le interesa percibir al robot (= buscar en los sensores,
buscar en las imágenes) no son siempre las mismas, dependen del estado en el que se encuentre. En cada
estado habrá una percepción necesaria para decidir qué ordenar a los actuadores (percepción para control)
y una percepción necesaria para verificar las transiciones posibles (percepción para transiciones).
Figura 29: Baliza arlequinada
En esta práctica interesa detectar la baliza de colores situada en el techo del coche. Tal y como es la
baliza (Figura 29), se pueden detectar filtrando por colores la imagen de la cámara ventral y verificando
ciertas condiciones espaciales. El filtro en el espacio HSV es más robusto y convendrá filtrar por verde y
por naranja. También es recomendable segmentar, es decir, agrupar los píxeles cercanos del mismo color.
La baliza hace que cuatro grupos de color aparezcan en la imagen, en posiciones arlequinadas. Puede haber
41
objetos de color verde o naranja también, pero es muy probable que aparezcan cerca y arlequinados en
cosas que no son la baliza. Esa configuración es muy discriminante.
Cuando el robot está lejos se observará la baliza completa, las cuatro zonas. En ese caso interesa tomar
como referencia para el control la cruceta, la frontera entre las cuatro zonas coloreadas. Si el robot está
cerca (típico del estado aterrizaje) puede que veamos la baliza parcialmente, con sólo una mancha de un
color, o dos, o tres. En todos los casos interesa decidir hacia dónde debe moverse el drone para corregir.
La propia sombra del cuadricóptero puede dificultar ligeramente el filtro de color.
Figura 30: Vistas parciales de las balizas o lejos, posibles a bordo
8.4.
¿Cómo hacerlo más divertido?
La primera manera es hacer que el coche se mueva rápidamente. Otra línea puede ser quitar la baliza,
eliminarla por completo del coche y que fuera un vehículo sin modificar.
42
9.
Práctica 6: Laberinto con flechas
Programa tu cuadricóptero para que sea capaz de salir de un laberinto. Para ayudarle se han puesto
flechas en el suelo que marcan la dirección a tomar en cada encrucijada. Este ejercicio de control visual
está diseñado para practicar la identificación de objetos mediante visión.
Los objetos en el escenario son flechas de colores, inscritos entre los cuatro vértices de un rectángulo.
Para la detección se propone la detección de los vértices, la transformación homográfica y la correlación
entre alguno de los patrones ideales (flecha hacia delante, hacia atrás, a la izquierda o a la derecha).
9.1.
Laberinto con flechas en el suelo/paredes.
gazebo ArDrone_labyrinth . world
Figura 31: Mundo con paredes y flechas de ayuda en el suelo
Para probar tu algoritmo en primer lugar tienes que lanzar el componente labyrinthEscape del
siguiente modo:
$ l a b y r i n t h E s c a p e / python main . py −−I c e . C o n f i g=i n t r o r o b _ p y _ s i m u l a t e d . c f g
Con labyrinthEscape en ejecución pulsta sobre el botón Take-off para despegar el drone y a
continuación sobre el botón Play para ejecutar tu algoritmo.
43
9.2.
Programando la detección de objetos
Hay muchas formas de identificar en las imágenes las flechas. La que te proponemos aquí tiene tres
pasos: (a) Detección de las cuatro esquinas, (b) homografía para rectificar la imagen y (c) correlación
entre la imagen rectificada y los patrones de flechas almacenadas. Para simplificar la percepción autónoma
hemos envuelto a cada flecha en un rectángulo imaginario con unas balizas azules en cada esquina.
El primer paso consiste en aplicar un filtro de color para detectar esas esquinas. Para saber si nos
encontramos ante una flecha tendremos que encontrar 4 regiones que formen un cuadrado, para ello
proponemos utilizar la clase SimpleBlobDetector de OpenCV, que nos devolverá las regiones encontradas
una vez realizado el filtro de color.
Figura 32: Flecha que sirve de guía
Cuando encontremos 4 regiones que formen aproximadamente un cuadrado, podremos rectificar la
imagen utilizando las funciones getPerspectiveTransform y warpPerspective. Esto nos permitirá
obtener una imagen de 100x100 píxeles que sea comparable con los patrones de flechas previamente
almacenadas.
El tercer paso consiste en comparar la imagen rectificada con los cuatro patrones existentes (la flecha
mirando al norte, al sur, al este o al oeste). Esa comparación se puede realizar de muchas maneras diferente.
La más simple es una correlación píxel a píxel con cada uno de los patrones. Aquel patrón que sea más
parecido dará la orientación indicada por la flecha observada.
Desde la percepción en la imagen hay que traducir a orientación espacial absoluta. El frente del robot
(eje rojo en el cuerpo del drone en el simulador) apunta siempre hacia las zonas altas de la imagen.
Dependiendo de cómo arranque el drone en el mundo y lo que le hagais girar, tened en cuenta esto para
traducir orientación de la flecha dentro de la imagen a orientación absoluta de la flecha y la desviación
respecto del frente del drone.
44
Figura 33: Ejemplo de rectificación de imágenes
9.3.
Programando el control
El control consiste en tener como referencia la orientación espacial objetivo indicada por la última
flecha. Esa orientación se puede conseguir con un control reactivo PID que mide en todo momento la
orientación del robot marcada por la brújula (interfaz pose3D) y la compara con la orientación absoluta
deseada. En función de ese error ordena a los motores del drone giros y avances.
9.4.
Si las flechas no están en el suelo, sino que pueden aparecer en las paredes la percepción. En la
misma línea, otra opción es utilizar la cámara frontal en vez de la ventral. La homografía debe funcionar
igualmente.
Otra cuestión a abordar para hacerlo más robusto es contemplar la situación en la que se puedan ver
varias flechas en la imagen. Habrá que distinguirlas todas y hacer caso a la más cercana (la más grande en
imagen).
45
10.
Práctica 7: Rescate de personas
Programa tu cuadricóptero para que sea capaz de explorar dentro de cierto perímetro buscando las
personas que existan. Para cada una de las personas que detecte debe hacer una foto, etiquetarla con la
posición en la que se encuentra (el propio drone). La zona de búsqueda se le proporciona en forma de una
secuencia de posiciones 2D que marcan el perímetro.
Este ejercicio está diseñado para practicar la navegación exploratoria y la detección visual de caras
humanas. Un posible uso de estas técnicas (no tan simplificadas, sino un poquito más sofisticadas y
robustas) es en aplicaciones de rescate. Imagina un escenario donde ha habido un accidente, por ejemplo
en el mar y hay personas flotando. O se han perdido montañeros y por una zona de difícil acceso.
10.1.
gazebo ArDrone_rescue−p e o p l e . world
Figura 34: Mundo con las personas a rescatar
46
Para probar tu algoritmo en primer lugar tienes que lanzar el componente rescuePeople del siguiente
modo:
$ r e s c u e P e o p l e / python main . py −−I c e . C o n f i g=i n t r o r o b _ p y _ s i m u l a t e d . c f g
Con rescuePeople en ejecución pulsta sobre el botón Take-off para despegar el drone y a continuación
sobre el botón Play para ejecutar tu algoritmo.
10.2.
Navegar para explorar
Se proporciona el perímetro de la zona donde hay que buscar potenciales víctimas. Se especifica como los
vértices 2D de un polígono genérico, de los cuales se dan sus coordenadas. Hay que diseñar un algoritmo
de navegación de modo que el drone explore esa zona en búsqueda de cualquier persona en ella. Este
algoritmo puede ser aleatorio, en espiral, o hacerlo más eficiente si se realiza de modo sistemático. Este
tipo de navegación resulta muy útil, por ejemplo, en las aspiradoras robóticas (Figura 35. En ese caso no
hay que explorar sino recorrer todo el espacio para limpiarlo.
Figura 35: Navegación exploratoria
El drone parte de una base, se acercará a la zona de búsqueda, realizará la exploración durante un cierto
intervalo (3 minutos por ejemplo) y retornará a la base con los resultados de su búsqueda: una persona (se
adjunta la foto tomada de ella) en tales coordenadas, otra (se adjunta su foto) en tales otras coordenadas,
etc... Te puedes apoyar en un autómata de estados para materializar estas fases del comportamiento
deseado.
10.3.
Detección de personas
Para simplificar este paso, en vez de complejas percepciones de personas vamos a suponer que las
víctimas están tendidas en el suelo bocarriba (Figura 36). El algoritmo de detección de personas que
vamos a desarrollar en el fondo será un algoritmo de detección de caras humanas (Figura 37). Puedes
desarrollar el algoritmo de detección que quieras, pero recomendamos utilizar el que viene en OpenCV con
el clasificador en cascada Haar de la clase CascadeClassifier20 .
20 http://docs.opencv.org/2.4/doc/tutorials/objdetect/cascade_classifier/cascade_classifier.html
47
Figura 36: Victimas tendidas en el suelo
Figura 37: Detección de caras
10.4.
Una manera de hacerlo más realista es detectar víctimas en cualquier orientación. Eso va a ralentizar
un poco el algoritmo y por eso hemos preferido no incluirlo en la versión básica de esta práctica.
Otro punto enriquecedor es que el drone no anote su propia posición cuando detecta una víctima, sino
que estime la posición absoluta de la víctima en 3D y la anote. Para ello tendrá que tener en cuenta que
la víctima puede no aparecer centrada en la imagen, y que además el drone estará a cierta altura. Para
resolverlo habrá que intersecar el plano suelo con el rayo de retropoyección desde el píxel donde detectemos
a la persona, teniendo en cuenta la propia posición 3D absoluta del drone para calcular la posición absoluta
de la víctima.
48

Entorno de prácticas del Curso Superior Universitario en

Transcripción

Documentos relacionados

NÚMERO DE ROBOS Total robos 240 229 METROS ROBADOS

delta drone lista de pre vuelo