3-Entorno de Simulaciã N

Capítulo 3
Entorno de simulación
ROS/Gazebo
En este capítulo se hará primero una introducción a ROS, en la que se

tratará de ver qué es ROS, y cuales son su filosofía, arquitectura, concep-
tos básicos, librerías de cliente y herramientas más comunes, profundizando
en aquellos conceptos y librerías que se han utilizado a lo largo del proyecto.
Tras esto, nos centraremos en el simulador que se va a utilizar dentro

de ROS, denominado Gazebo, para crear y realizar simulaciones con los
modelos de nuestros quadrotor y nuestro manipulador. Se comentarán su
entorno, configuración, plugins y diversas posibilidades que ofrece.
Todo el contenido de este capitulo es una recopilación de la información

obtenida de la documentación de la propia web de ROS[9] y de realizar
las decenas de tutoriales que ofrece ROS para comenzar1 y profundizar2
en el uso del propio Meta-Sistema Operativo así como en los conceptos,
herramientas y librerías particulares.
3.1 ROS
3.1.1 Introducción a ROS
Robot Operating System (ROS) es una plataforma software, lo que se
conoce como framework, para el desarrollo de software específico para ro-
bótica. Fue creado por el instituto de investigación Willow Garage en 2007
bajo licencia BSD y todos sus programas son de código abierto (OSS), per-
mitiéndose así su uso gratuito tanto para la investigación como para fines
comerciales.
Su filosofía es la de que todos los programas sean de uso libre, reutiliza-

bles, escalables según la aplicación deseada e integrables con todo el software
de robótica existente y futuro. Por ello el sistema se creó teniendo en cuenta
1
http://www.ros.org/wiki/ROS/Tutorials
2
http://www.ros.org/wiki/AllTutorials
65
66 ENTORNO DE SIMULACIÓN ROS/GAZEBO
otras plataformas software abiertas ya existentes como OpenCV, Player/S-

tage, Gazebo, Orocos/KDL y otros, existiendo paquetes para su uso en ROS.
ROS se compone de un conjunto de librerías de programación, aplica-

ciones, drivers y herramientas de visualización, monitorización, simulación
y análisis, todas reutilizables para el desarrollo de nuevas aplicaciones para
robots tanto simulados como reales.
Por otro lado, ROS nos proporciona también servicios estándares propios
de un sistema operativo pero sin serlo, ya que se instala sobre otro, en
general Linux y de manera recomendada Ubuntu, y por ello también recibe
la denominación de Meta-Sistema Operativo. Posee su propio manejador de
paquetes mediante comandos desde terminal para la gestión, compilación y
ejecución de archivos, así como abstracción de hardware[10].
Figura 3.1: Relación ROS y SO
Su estructura es modular, de forma que cada programa o aplicación,

lo que en ROS se denomina como nodo, se ejecuta dentro otro ejecutable
(Master) que funciona de núcleo del sistema y hace las veces de plataforma
por la que se comunican los diferentes nodos mediante topics y/o servicios,
de los que se hablará más adelante.
Para comprobar que ROS representa un sistema completo de desarrollo

de software para robots, basta con observar el trabajo realizado con el robot
PR2, mostrado en la Figura 3.2, cuyo sistema software ha sido desarrollado
por Willow Garage únicamente usando ROS. Además todas las capacida-
des del PR2 (visión, movimiento de la base y de los brazos, etcétera) están
disponibles vía ROS, por lo que podemos utilizar y programar el robot a
través de ROS.
Esto último hace que algunos paquetes de ROS sean en principio espe-
cíficos para este robot, pero, como se verá, una gran parte de ellos pueden
adaptarse y utilizarse a cualquier otro robot real o simulado, siempre que
ROS 67
Figura 3.2: Robot PR2
cumplamos con las interfaces necesarias.
Dado que ROS está continuamente evolucionando, existen varias distri-

buciones o versiones3 :
ROS Fuerte (Abril, 2012)
ROS Electric (Agosto, 2011)
ROS Diamondback (Marzo, 2011)
ROS C Turtle (Agosto, 2010)
ROS Box Turtle (Marzo, 2010)
En este proyecto se ha utilizado la distribución Electric, dado que era la

más actual cuando comenzó, y se ha instalado sobre Ubuntu 11.04 (Natty).
Esto es muy importante, dado que no se asegura el perfecto funcionamiento
de la aplicación ni la existencia de los paquetes necesarios para distribu-
ciones anteriores de ROS. Además, ha de tenerse en cuenta que el único
sistema operativo en el que se asegura la compatibilidad con ROS es Ubun-
tu, estando los demás, y Windows en particular, en fase experimental.
Por último, ha de comentarse la existencia de un foro oficial de ROS

(http://answers.ros.org) con una alta participación tanto de investiga-
dores como de usuarios particulares y que ha servido de gran ayuda durante
la elaboración de este proyecto.
3.1.2 Conceptos básicos

Sistema de archivos
El sistema de archivos de ROS se divide básicamente en dos niveles[10]:
1. Paquetes (packages): Son el nivel más bajo de organización del sistema

de archivos de ROS. Pueden contener cualquier cosa: ejecutables (no-
dos), modelos, tipos de mensajes y servicios, herramientas o librerías.
3
http://www.willowgarage.com/pages/software/ros-platform
Figura 3.3: Paquetes
2. Pilas (Stacks): Son agrupaciones de paquetes que forman una librería

de alto nivel. Cada pila agrupa paquetes que son complementarios
entre ellos.
Figura 3.4: Pilas
Ambos poseen unos archivos de información especificando lo que contie-

nen, su funcionalidad y los paquetes de los que depende. En las pilas éste
archivo se denomina stack.xml y en los paquetes manifest.xml. Esto es una
forma rápida de diferenciar cuando si estamos dentro de un paquete o si
estamos dentro de un pila.
A la hora de ser descargados e instalados, las pilas se agrupan en repo-

sitorios tal y como se muestra en la Figura 3.5, que son equivalentes a los
repositorios de Linux. De hecho los repositorios de ROS pueden ser descar-
gados equivalentemente a los de Linux, directamente por línea de comandos
o mediante el administrador de descargas.
Figura 3.5: Repositorios
Por defecto, todo el sistema de ROS se instala en la dirección /opt/ros

estando restringidos los derechos de modificación de sus paquetes para evi-
tar dañar el sistema. Por ello, en general y en el caso de este proyecto, se
crea una carpeta de trabajo (ros_workspace) para crear nuestras aplicacio-
nes en ella y que se añade al path de ROS para que éste tenga acceso.
ROS 69
Para entender más adelante como se organizan los paquetes de nuestras

aplicaciones, será conveniente explicar los tipos de carpetas y archivos que
podemos encontrarnos normalmente, sin entrar en detalles de cada uno, en
un paquete de ROS:
Carpeta bin: contiene los ejecutables del paquete, los que se podrán
lanzar como nodos.
Carpeta build : contiene los archivos de compilación internos del pa-
quete.
Carpeta src: contiene los códigos de los programas (.cpp), que al com-
pilarlos crearán ejecutables en bin.
Carpeta include: contiene los archivos de cabeceras (.h) propios de los
programas del paquete.
Carpeta lib: contiene los archivos de librerías (.lib, .so).
Carpeta launch: contiene los archivos de lanzamiento (.launch) de apli-
caciones completas.
Carpeta yaml : contiene los archivos de lista de parámetros (.yaml).
Carpeta msg: contiene los tipos de mensajes (.msg) definidos en el
paquete.
Carpeta msg_gen: contiene los archivos de cabeceras (.h) auto-generadas
al compilar los tipos de mensajes definidos en el paquete.
Carpeta srv : contiene los tipos de mensajes de los servicios definidos
en el paquete.
Carpeta srv_gen: contiene los archivos de cabeceras (.h) auto-generadas
al compilar los tipos de servicios definidos en el paquete.
Archivo CMakeLists.txt: es una lista en la que se especifica al compila-
dor que debe compilar de nuestro paquete: nodos, mensajes, servicios,
librerías, etc.
Archivo manifest.xml : contiene una descripción del paquete (función,
autor, licencia, etc.) y una lista con los paquetes de los que depende.
Según la funcionalidad del paquete, es posible encontrarnos más o menos
carpetas con archivos diferentes. En el caso más común de un paquete que se
usa para simulación en Gazebo, como los que se van a crear en este proyecto,
nos encontraremos además de todas o casi todas las anteriores algunas como
estás:
Carpeta visual : contiene los archivos gráficos de mallas (.dae) que se
usan para el modelo visual y de colisión.
Carpeta robots: contiene los archivos de descripción de los robots
(.urdf, .xml), para sus modelos en Gazebo y rviz.
Compilación
Para compilar un paquete en ROS se utiliza CMake, una plataforma de
código abierto para la generación, compilación y comprobación de paquetes
de software. Su uso es bastante sencillo, basta con que el paquete de ROS
contenga un archivo llamado CMakeLists.txt compuesto por una serie de
macros según lo que se quiera crear y compilar4 .
Obviando aquellas que por defecto aparecen para lanzar CMake y confi-
gurar la ubicación predeterminada para las salidas, las macros más comunes
que se utilizan son las siguientes:
rosbuild_add_executable(ejecutable src/programa.cpp): crea en la car-

peta bin el ejecutable de programa.cpp descrito en src.
rosbuild_add_library(librería src/programa.cpp): crea en la carpeta lib
la librería de programa.cpp descrito en src.
rosbuild_genmsg(): auto-genera todos las cabeceras y archivos nece-
sarios de los tipos de mensaje definidos en la carpeta msg del paquete
y nos los guardará en una nueva carpeta denominada msg_gen.
rosbuild_gensrv():auto-genera todos las cabeceras y archivos necesa-
rios de los tipos de servicio definidos en la carpeta srv del paquete y
nos los guardará en una nueva carpeta denominada srv_gen.
Para compilar un paquete, basta con hacer en una terminal:
$ rosmake nombre_paquete
Ésto compilará todo lo especificado en CMakeLists.txt, además de to-

dos los paquetes de los que depende éste, es decir, aquellos especificados en
manifest.xml de la forma:
<depend package=“nombre_paquete”>
Nodos
Un nodo es un módulo o proceso individual dentro del sistema de ROS
que realiza un cómputo, que puede enviar y/o recibir información de otros
nodos y usar y/u ofrecer servicios5 . Estos nodos no son más que los ejecuta-
bles de los paquetes de ROS ejecutándose. En la Figura 3.6 se muestra un
ejemplo de dos nodos, talker y listener en el que el primero está publicando
en un topic, denominado chatter, y el segundo está subscrito a él.
Cada nodo posee un nombre único, que lo identifica y lo distingue de
todos los demás nodos en ejecución. Así por ejemplo es posible lanzar el
4
http://www.ros.org/wiki/rosbuild/CMakeLists
5
http://www.ros.org/wiki/Nodes
ROS 71
Figura 3.6: Nodos
mismo programa dos veces, lo que da lugar a dos nodos, siempre que éstos
tengan diferente nombres.
Los nodos se pueden escribir en C++ o en phyton usando su librería de

cliente respectiva que ROS nos ofrece, que a su vez son paquetes de ROS:
roscpp: librería de cliente de C++.

rospy : librería de cliente de phyton.
Es muy importante tener en cuenta que la ejecución y comunicación

entre nodos es independiente del lenguaje que en que estén escritos, funcio-
nará correctamente siempre y cuando se cumplan las interfaces entre ellos,
es decir, se envíen y reciban siempre los mensajes del mismo tipo.
Los programas de los nodos pertenecientes a este proyecto se escribirán

todos en C++, utilizando el roscpp 6 . Este paquete es una implementación
del lenguaje de programación C++ específico para ROS. Contiene las libre-
rías básicas del lenguaje y otras que nos proporcionan diferentes funciones y
utilidades además de las interfaces para utilizar los topics, servicios y pará-
metros de ROS para comunicarnos desde el programa con los demás nodos
y aplicaciones que se estén ejecutando en el sistema.
La programación de todos los nodos en ROS debe incluir las siguientes

instrucciones:
#include "ros/ros.h"
int main(int argc, char **argv)

{
ros::init(argc, argv, "nombre_nodo");
ros::NodeHandle n;
(...)
return 0;
}
“ros.h” contiene todas las cabeceras necesarias para el uso de las instruc-
ciones más comunes del sistema de ROS, como por ejemplo las referentes al
uso de los topics y servicios, pero no incluye los tipos de mensajes y otras
6
http://www.ros.org/wiki/roscpp
instrucciones.
“ros::init(argc, argv, “nombre_nodo”)” inicializa el nodo en el sistema con

el nombre que le asignemos y los argumentos que haya recibido al ejecutarlo.
“ros::NodeHandle n” crea el principal punto de acceso a las comunicacio-

nes de este nodo con el resto del sistema en ROS. Es una interfaz para crear
suscripciones y/o publicaciones del nodo a los topics, así como para acceder
al servidor de parámetros. Además, junto con init(), inicializa el nodo y lo
elimina automáticamente al finalizar la ejecución del programa. “n” sería el
identificado del proceso de este nodo y debe ser único.
Para ejecutar ciclos a una frecuencia deseada, por ejemplo para recibir
mensajes cada cierto tiempo ó controlar un robot, roscpp ofrece las siguien-
tes instrucciones:
ros::Rate r(10);
while (ros::ok()))
{
(Código a ejecutar...)
r.sleep();
}
“ros::Rate r(10)” permite especificar la frecuencia a la que se va a eje-
cutar el bucle while(). En este caso, se habría elegido una frecuencia de 10
Hz.
“ros::ok()” es una función que devuelve un valor falso cuando se dé se

reciba un [ctrl.+C ], haya sido expulsado el nodo por que el nombre ya existe
ó “ros::shutdown()” haya sido ejecutada (por ejemplo por NodeHandle).
“r.sleep()” es la función que nos asegura que se cumpla el ciclo en 10 Hz

esperando hasta el fin de éste.
Topics
Los topics son buses por los cuales los nodos envían o reciben mensa-
jes. Simplemente un nodo debe comunicar al Master que desea publicar en
él para enviar información o suscribirse en él para recibir su información.
Pueden existir varios nodos publicando y varios nodos subscritos a la vez
en un mismo topic. Son anónimos, en el sentido de que los nodos publican
ó se suscriben a ellos pero nunca saben que otros nodos están publicando
o subscritos en el topic, lo cual desacopla la producción del consumo de la
información7 .
Todos los topics son unidireccionales, por lo cual un nodo que envía in-
formación por un topic nunca recibirá una respuesta directamente por el
7
http://www.ros.org/wiki/Topics
ROS 73
Figura 3.7: Topic[1]
mismo topic ni sabrá si sus mensajes están siendo recibidos. Si se desea

enviar una información y recibir una respuesta debe usarse el otro tipo de
comunicación entre nodos, los servicios.
Por un topic se envían objetos de un tipo de mensaje definido en el mis-

mo paquete u en otro paquete del que dependa el paquete en cuestión. La
definición de estos mensajes-objetos se incluyen en el programa mediante
las cabeceras generadas en los paquetes a partir de los archivos .msg, de los
que se hablará más adelante.
Un topic viene identificado por su nombre, que debe ser único y nor-
malmente es de la forma: “/nodo_que_lo_publica/nombre_topic”, aunque
esto es decisión del programador. A la hora de su declaración, además de
éste nombre, es necesario definir el tipo de mensaje que se va a enviar por
ellos y por tanto el tipo de mensaje que se va a recibir por ellos. El Master
no asegura esta concordancia, por lo cual ha de tenerse especial cuidado en
que al subscribirnos a un topic se haga con el tipo de mensaje que se está
enviando por éste.
Las funciones básicas que roscpp nos ofrece para anunciar que se va
publicar por un topic (creándose éste automáticamente si no existía) y
enviar datos son las del siguiente ejemplo:
#include "std_msgs/String.h"
(...)
ros::Publisher chatter_pub = n.advertise<std_msgs::String

>("chatter", 1000);
std_msgs::String msg;
msg.data = "hello world";
chatter_pub.publish(msg);
La primera instrucción incluye las definiciones de la clase del tipo de

mensaje que se vaya a utilizar, en este caso String del paquete std_msgs.
Con la segunda instrucción se crea el topic “chatter ”, con una cola de capa-
cidad de 1000 mensajes, se define el tipo de mensaje que se va a enviar,
“std_msg::String”. Esta instrucción nos devolverá un objeto de la clase
“ros::Publisher”, en este caso denominado “chatter_pub”, que sirve como
identificador del topic dentro del programa y posee la función publish()
que es la que se usa en la última instrucción para enviar el mensaje por
el topic desde el programa (o nodo cuando esté en ejecución). La terce-

ra es simplemente un objeto que tendrá la estructura del tipo de mensaje
“std_msgs::String” (simple cadena de caracteres) y que es la que se va a
enviar.
Aunque no se ha dicho, es obvio que el codigo anterior, en general, irá

dentro de la función main().
La suscripción a un topic es equivalente:
#include "std_msgs/String.h"
(...)
void chatter_funcion(const std_msgs::String msg)

{
ROS_INFO("Recibida cadena: [ %s]", msg.data.c_str());
}

{
ros::Subscriber sub = n.subscribe("chatter", 1000,
chatter_funcion);
(...)
}
Con la última instrucción el nodo se suscribe al topic “chatter” y se
declara que, al recibirse un mensaje por el topic, se llamará a la función
“chatter_funcion”, que debe estar definida en el mismo programa y que,
como puede observarse, recibe como argumento el mensaje que llegue por
el topic. La función ROS_INFO(), que se explicará más adelante, es equi-
valente a printf().
La instrucción anterior para la suscripción al topic, no hace por sí sola

que se reciban los mensajes. Para ello ha de añadirse otra tipo de función
en el punto del programa donde quiere que se reciba. Existen varias posibi-
lidades:
ros::spin(): Función que mantendrá nuestro programa en un bucle
en el que se espera a que lleguen mensajes por el topic y se llamará
a la función “chatter_funcion” lo más rápido posible. Terminará una
vez que ros::ok() devuelva un valor falso, es decir, una vez que se ha
llamado a ros::shutdown, ya sea por un [ctrl.+C ] o por que el Master
de ROS se apague.
ros::spinOnce(): Se recibirán todos los mensajes que estén en cola
en todos los topics en esté punto de la ejecución. Luego el programa
seguirá ejecutándose normalmente.
Ambas instrucciones anteriores son para hilos únicos. Existen otras po-
sibilidades para hilos múltiples, que pueden encontrarse en la wiki de ROS,
ROS 75
pero que no se han usado en este proyecto.
Típicamente, ros::spinOnce()"siempre vá dentro de un bucle while() co-

mo el que se comentó en el apartado anterior, pero obviamente ros::spin()"no.
Servicios
Los servicios en ROS son la forma en que se envía un mensaje (request)
desde un nodo a otro y éste le responde con otro mensaje (response):
Figura 3.8: Servicio[1]
Así un nodo puede ofrecer un servicio (server ) y cualquier otro nodo

puede utilizarlo (client) llamándolo junto con un mensaje y esperando una
respuesta de éste, es decir, otro mensaje.
El servicio ofrecido por un nodo es totalmente independiente de los clien-

tes. Es posible, por ejemplo, que un nodo ofrezca un servicio y el usuario
llame a este servicio desde una terminal.
Los servicios vienen definidos mediante los archivos .srv, que definen el
tipo de servicio que se va a ofrecer, esto es el tipo de mensaje que se va a
recibir como request y el tipo de mensaje que se va a devolver como respon-
se. Al igual que con los mensajes para los topics, ahora se deben incluir las
cabeceras auto-generadas por los archivos .srv del paquete en cuestión, que
contendrán las definiciones de las clases del tipo de servicio8 .
Las funciones básicas que roscpp nos ofrece para crear y ofrecer un ser-
vicio son las del siguiente ejemplo:
#include "Matematicas/SumaDosEnteros.h"
bool fun_sum(Matematicas::SumaDosEnteros::Request &req,

Matematicas::SumaDosEnteros::Response &res)
{
res.sum = req.a + req.b;
Return true;
}
{
(...)
8
http://www.ros.org/wiki/Services
ros::ServiceServer service = n.advertiseService("

suma_dos_enteros",fun_sum);
(...)
}
La primera instrucción incluiría el tipo de servicio que se va a usar,

SumaDosEnteros.srv del paquete Matematicas, que define la clase del tipo
de servicio. La última crea propiamente el servicio, devolviendo su identi-
ficador dentro del programa “service”. El nombre del servicio ofrecido será
“suma_dos_enteros” y la función que ejecutará el servicio al ser llamado
será “fun_sum”, que debe también definirse en el programa. Como puede
observarse, la función recibe como argumentos las direcciones donde se en-
cuentra el mensaje recibido (request) y donde debe escribir la respuesta
(response). En este caso el archivo .srv, que define el tipo de mensaje que
se envía y el tipo de mensaje que se recibe sería9 :
SumaDosEnteros:
int64 a
int64 b
—
int64 sum
Las dos partes del servicio, request y response, siempre vienen separadas
por el marcador “—”.
Equivalentemente para usar un servicio creamos un cliente a éste de la

siguiente forma:
int main(int argc, char **argv) {

ros::ServiceClient client = n.serviceClient<Matematicas
::SumaDosEnteros>("suma_dos_enteros");
beginner_tutorials::AddTwoInts srv;
srv.request.a = 10;
srv.request.b = -3;
client.call(srv)
return 0;
}
Lo primero, en el código anterior, es incluir de nuevo el archivo de cabe-

cera del tipo de servicio. La segunda instrucción crea el cliente al servicio
“suma_dos_enteros” que ofrecería el otro nodo y se le define también el
tipo del servicio, obteniendo el identificador “client” para llamar al servicio.
9
http://www.ros.org/wiki/srv
ROS 77
La tercera sería un objeto de la clase del servicio, que contiene los miem-
bros request y response. Por último se rellena el objeto y se envía usando
la función “call()” del identificador.
Mensajes
Como ya se ha comentado, los nodos se comunican entre sí publicando y
recibiendo mensajes vía topics. Los mensajes son simplemente unas estruc-
turas de datos equivalentes a una estructura de C++ que se definen en la
carpeta msg de los paquetes mediante archivos de texto de extensión .msg
en forma de lista tipo-nombre10 :
Mensaje:
type1 name1
type2 name2
type3 name3
La estructura puede estar formada por primitivas estándares equivalen-

tes a las de C++, definidas en el paquete std_msgs y que se describen en
la tabla 3.111 .
Tabla 3.1: Equivalencia de tipos de datos
msg C++
bool uint8_t
int8 int8_t
uint8 uint8_t
int16 int16_t
uint16 uint16_t
int32 int32_t
uint32 uint32_t
int64 int64_t
uint64 uint64_t
float32 float
float64 double
string std::string
time ros::Time
duration ros::Duration
También pueden estar formados por otros tipos de mensajes (.msg) es-
tándares o ya definidos en std_msgs o en cualquier otro paquete de ROS,
siempre y cuando exista la dependencia oportuna entre paquetes. Por ejem-
10
http://www.ros.org/wiki/Messages
11
http://www.ros.org/wiki/msg
plo, el tipo de mensaje Header, del paquete std_msgs, se utiliza como enca-
bezamiento en multitud de tipos de mensajes y es estructura es la siguiente:
Header:
uint32 seq
time stamp
string frame_id
En orden descendente, representan el número dentro de la secuencia de

mensajes por el topic, el instante de tiempo en que se envió y el SR asocia-
do, en su caso.
Por otro lado, se comentó que para los servicios se definían sus tipos en
los archivos .srv. Estos archivos son equivalentes a los .msg salvo en que se
componen de dos listas separadas por “—”, primero la lista de la estructura
de request y luego la de response. Obviamente estos archivos .srv se guardan
en su carpeta srv para que el compilador los encuentre y pueda crear sus
archivos de cabecera.
Es posible crear variables de tipos de mensajes o servicios dentro del pro-

grama (en C++) simplemente incluyendo los archivos de encabezamiento
generados por estos y tratándolos igual que si fuera una variables o estruc-
tura u objeto más. Por ejemplo, si se crea una del variable del tipo Header,
para acceder a su variable miembro stamp se haría: “header.stamp”.
Master
El Master (o roscore) es un nodo que funciona de núcleo del sistema,
proporcionando una plataforma mediante el registro de nombres, servicios
y parámetros, sobre la cual se hace posible la comunicación y el envío de
datos entre los diferentes nodos individuales del sistema. Sin él los nodos no
se “encontrarían” entre ellos.
Figura 3.9: Master[1]
Realmente roscore es una herramienta que proporciona tres cosas:
El Master, entendido como plataforma de comunicación para los nodos.
El Servidor de Parámetros.
Registro de salida al que todos los nodos envían información denomi-

nado /rosout.
ROS 79
Como ejemplo, véase en la Figura 3.10 como los dos nodos, uno (cámara)
que envía imágenes y otro que las recibe, se dirigen antes al Master para
crear y buscar, respectivamente, el topic antes de iniciar la comunicación
entre ellos12 .
Figura 3.10: Ejemplo funcionamiento del Master
Respecto al servidor de parámetros, éste es utilizado por los nodos

para almacenar o leer parámetros en tiempo de ejecución, haciendo además
posible que el usuario puede ver y modificar estos parámetros por terminal
también en tiempo de ejecución.
Como ya se puede prever, la ejecución del Master es obligatoria antes de

lanzar nodos que se comuniquen entre ellos o necesiten leer parámetros, es
decir, siempre. Para ello basta con lanzar por terminal:
$ roscore
Aunque algunos programas ya lo lanzan al ser ejecutados, no siempre es

así, por lo cual se aconseja que siempre se haga roscore antes de empezar.
Lanzadores
Como veremos en el siguiente apartado, es posible ejecutar nodos, llamar
a servicios y leer o publicar en topics directamente desde una terminal. Sin
embargo, si lo que se desea es lanzar varios nodos a la vez, cargar una
serie de parámetros o ejecutar varias herramientas, esa no sería la mejor
opción ya que habría que hacerlo uno por uno. Por esta razón existe en ROS
una herramienta denominada roslaunch, que permite fácilmente lanzar
múltiples nodos con sus argumentos, establecer una serie de parámetros en
el servidor y otras opciones bastante interesantes directamente tomándolos
de unos archivos de configuración de extensión .launch. Así simplemente
habría que lanzar desde una terminal:
$ roslaunch nombre_paquete nombre_archivo.launch
Los lanzadores .launch son archivos de configuración escritos en XML13

en los que se especifican los nodos a lanzar, los argumentos de entrada a
éstos y los parámetros necesarios para crear una aplicación completa. Como
todo archivo XML, se compone de una serie de etiquetas (tags) que tienen
12
http://www.ros.org/wiki/Master
13
http://ros.org/wiki/roslaunch/XML
una serie de opciones a rellenar. A continuación se comentan las etiquetas

más comunes y su utilidad:
<launch>: Es el primer elemento de cualquier archivo .launch. Es sim-

plemente un identificador del tipo de archivo del que se trata. Siempre
se encontrará al principio y al final en formato de cierre </launch>.
<node>: Se utiliza para lanzar un nodo. En principio, el nodo es una
copia del nodo “tipo” que se quiere lanzar por lo que hay que darle un
nombre diferente y especificar el paquete y el nombre del nodo “tipo”:
<node pkg=“tortuga_pkg” name=“tortuga” type=“tortuga_node”

args=“-v 10” respawn=“true”/>
Los dos últimos argumentos son opcionales.

<include>: Permite incluir otro archivo launch (.launch o .xml ) a par-
tir de su ruta:
<include file=“$ (find paquete que lo contiene)/ ruta hacia él/ achi-
vo.xml”>
<param>: Permite definir un parámetro en el Servidor de Parámetros.
Este parámetro puede ser un valor numérico constante:
<param name=“P” value=“3.4”>
O una cadena de caracteres en la que se almacena el contenido de un

archivo:
<param name=“robot_description” command=“$(find quadrotor/ro-

bots/quardrotor.urdf )”>
En este caso se estaría guardando la descripción URDF del modelo del

quadrotor.
<arg>: Permite recibir argumentos al ejecutar el launcher y definirle
unos valores por defecto:
<arg name=“prueba” default=“true” />
Existen otros posibles tags pero no han sido usados en este proyecto.
Pueden verse en la wiki de ROS.
Herramientas de línea de comandos

Como Meta-Sistema Operativo que es, ROS tiene sus propias herra-
mientas de línea de comandos para la búsqueda de paquetes y archivos, la
ROS 81
compilación y depuración de código, la ejecución de programas y la obten-

ción de información del sistema en tiempo real. A continuación se presentan
los más usuales:
$ roscore: ejecuta el Master, el Servidor de parámetros y el nodo de

registro.
$ rosstack : obtener información sobre la pila que deseemos. Uso:
$ rosstack [comando] [stack]
$ rospack : obtener información sobre el paquete que deseemos. Uso:
$ rospack [comando] [paquete]
$ roscd : acceder a un paquete en cuestión. Uso:
$ roscd [paquete]
$ rosls: ver contenido del paquete en cuestión. Uso:
$ rosls [paquete]
$ roscreate-pkg: crear una paquete en el lugar donde nos encontremos,

con la posibilidad de incluir las dependencias hacia otros paquetes
directamente. Uso:
$ roscreate-pkg [nombre paquete] [dependencia1] . . .
$ rosmake: compilar el paquete deseado y todos de los que dependa.

Uso:
$ rosmake [paquete]
$ rosdep: descargar e instalar los paquetes de los que dependa el pa-

quete en cuestión. Uso:
$ rosdep install [paquete]
$ rosrun: ejecutar un nodo individual. Uso:
$ rosrun [paquete] [ejecutable]
$ rosnode: obtener la lista de nodos en ejecución, información de un

nodo o eliminarlo de la ejecución. Uso:
$ rosnode [comando] [nodo]

$ rostopic: obtener la lista de topics de los nodos en ejecución, infor-

mación sobre uno o leer/publicar en él. Uso:
$ rostopic [comando] [topic] [argumentos]
$ rosservice: obtener la lista de servicios de los nodos en ejecución,

información sobre uno o utilizarlo. Uso:
$ rosservice [comando] [servicio] [argumentos]
$ rosmsg: obtener el información de un tipo de mensaje. Uso:
$ rosmsg [comando] [tipo_mensaje]
Para obtener más información acerca de que comandos tiene cada he-
rramienta y su forma de uso, puede consultarse el archivo ROScheats-
heet.pdf [11] en la página inicial de ROS o, directamente desde una termi-
nal, podemos escribir:
$ [herramienta] -h
para obtener la forma de uso, los comandos posibles y sus utilidades, o:
$ [herramienta] [comando] -h
para obtener la forma para un comando concreto.
3.1.3 Aplicaciones y librerías de cliente

Como ya se ha comentado, ROS contiene una gran variedad de paque-
tes con diferentes funcionalidades y dos librerías de clientes, roscpp (para
C++) y rospy (para phyton)14 . Se presenta ahora una tabla con los stacks
y paquetes que se han usado en este proyecto que nos ofrecen una API en
C++ para el uso de las diferentes funcionalidades, así como los que nos
ofrecen una integración en ROS:
Tabla 3.2: APIs para C++
Utilidad ROS C++

Topics, servicios y tiempo ROS (roscore) roscpp
Tipos de mensajes common_msgs common_msgs
Sistemas de Coor. y de Ref. — TF, robot_state_publisher
Modelado de Robots — robot_model
Simulación 3D simulator_gazebo simulator_gazebo
Control en TR pr2_controller_manager pr2_controller_interface
Controladores — control_toolbox
14
http://www.ros.org/wiki/APIs
ROS 83
Para aclarar esta tabla, cabe especificar que en la columna de C++ se

listan los paquetes o pilas que nos ofrecen una API dentro de ROS, con
la utilidad correspondiente, para desarrollar nuestras propias aplicaciones,
es decir, que nos ofrecen una serie de bibliotecas con funciones, clases y/o
métodos para ser utilizados dentro de nuestros programas, y en la columna
de ROS aparecen los paquetes o pilas que nos ofrecen la integración del
paquete de la columna anterior en el sistema ROS.
Existen muchos más paquetes y pilas con diferentes utilidades, pero que
escapan del alcance de este proyecto.
A continuación se entrará en detalle dentro de cada uno de ellos, de forma

que todos los programas realizados para este proyecto queden cubiertos, en
la mayor medida posible, por estas explicaciones.
Robot Model - URDF

robot_model es un stack que contiene un conjunto de paquetes para el
modelado cinemático y dinámico de robots, así como un conjunto de herra-
mientas para su validación y su conversión en distintos formatos15 .
El paquete central del stack es el denominado urdf que posee un intér-

prete que permite especificar el modelo del robot en formato XML. A éste
modo de representación se le denomina URDF (Unified Robot Description
Format) y es compatible tanto con el visualizador rviz como con el simula-
dor Gazebo.
El formato de descripción de robots URDF consta de una serie de etique-

tas XML que permiten especificar una serie de parámetros de cada eslabón
y de cada articulación del robot. Aunque el modo descripción es práctica-
mente genérico para todo tipo de robots, desde un UAV hasta un brazo
manipulador, existen ciertas limitaciones que hacen imposible la descrip-
ción de ciertos robots. La principal limitación es que los robots deben ser
robots series (estructura de árbol), descartando todos los robots paralelos
o, dicho de otro modo, no siendo posible crear cadenas cinemáticas cerra-
das. Además, se asume que el robot se compone de eslabones rígidos unidos
por articulaciones, es decir, que no es posible ningún eslabón flexible. Aún
así y dado que los robots de interés en este proyecto cumplen los requeri-
mientos anteriores, el formato URDF es bastante interesante pues permite
especificar:
Descripción cinemática y dinámica del robot (eslabones y articulacio-

nes).
Representación visual del robot (de cada eslabón).
Modelo de colisión del robot (de cada eslabón).

15
http://www.ros.org/wiki/robot_model
A continuación se entrará en detalle en como describir un robot en for-

mato URDF16 . El robot se tratará como un conjunto de eslabones o cuerpos
(links) unidos mediante un conjunto de articulaciones (joints) tal y como se
muestra en la figura Figura 3.11, quedando una descripción global del tipo:
Figura 3.11: URDF: Links y joints
<robot name="manipulador">
<link> ... </link>
<link> ... </link>
<link> ... </link>
<joint> ... </joint>

</robot>
Cada articulación o elemento link describe un cuerpo rígido en cuanto a
su inercia, su forma visual y su forma de colisión, por lo que se compondrá de
tres bloques independientes entre sí, cada uno con un sistema de referencia
definido respecto al del joint antecesor, salvo en el caso del primer link que
será respecto al sistema de referencia visual de modelado de éste:
<link name="eslabon_1">
<inertial> ... </inertial>
<visual> ... </visual>
<collision> ... </collision>

16
http://www.ros.org/wiki/urdf
ROS 85
Figura 3.12: Links
</link>
Aunque luego se verán los joints, para aclarar la cuestión de las locali-
zaciones de los sistemas de referencias, hay que indicar que el sistema de
referencia del joint antecesor de cada link se encuentra siempre en el origen
visual del link, que situado en el punto donde se encontraba en el programa
que se modeló, en este caso Solid Edge, y que en la figura Figura 3.12 sería
el “link origen”, de color negro.
Dentro de cada bloque habrá que definir una serie de parámetros y es-
pecificaciones para el eslabón:
Dentro del bloque inercial:

<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="10e-4" ixy="0" ixz="0"
iyy="10e-4" iyz="0" izz="10e-4" />
</inertial>
• <origin>: Origen del sistema de referencia inercial. Obligatoria-

mente debe estar en el centro de masa del cuerpo. La orientación
es en representación roll, pitch e yaw en radianes.
• <mass>: Valor de la masa total del cuerpo.
• <inertia>: Matriz de inercia medida en el centro de masa y res-
pecto al este sistema de referencia y basta con 6 elementos (matriz
simétrica).
Dentro del bloque visual:

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1 1 1" />
</geometry>
<material name="Cyan">
<color rgba="0 255 255 1"/>
</material>
</visual>
• <origin>: Origen del sistema de referencia visual.

• <geometry>: Forma visual del cuerpo (box, cylinder o sphere). Es
posible sustituir esta forma por una malla (mesh) que tengamos
guardada en un archivo de extensión .dae, que incluso puede traer
la textura adjuntada, obviando así el siguiente tag de <material>.
Esto es lo que se hará en casi todos los links de los robots de este
proyecto, siendo necesario como se verá un reescalado:
<mesh filename="ruta_archivo/archivo.dae"
scale="0.0255 0.0255 0.0255" />
• <material>: Material visual del cuerpo, asignándole un color en

formato rgba, siendo la última letra referente a la transparencia
(α) entre 0 y 1.
Y por último, dentro del bloque de colisión:

<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="1" length="0.5"/>
</geometry>
</collision>
Cuyas especificaciones tienen significados equivalentes a los del bloque

visual, e igualmente es posible incluir una malla (mesh).
Téngase en cuenta que es posible definir un cuerpo cuyo bloque de coli-

sión sea totalmente diferente al bloque visual, por lo que se podrían hacer
cuerpos con una malla de colisión más grande al de la visual para tener en
cuenta posibles componentes que no se han incluido en el visual o, por el
contrario, hacer un cuerpo que nunca colisione porque no nos interese por
algún motivo.
Por otro lado, se describen las articulaciones o joints que especifican el

tipo de unión entre dos links, así como sus límites y propiedades dinámicas:
<joint name="art_12" type="revolute">
<origin xyz="0 0 1" rpy="0 0 3.1416"/>
<parent link="link_1"/>
<child link="link_2"/>
<axis xyz="0 0 1"/>
<dynamics ... />
<limit ... />
<safety_controller ... />
</joint>
En la etiqueta que abre el joint hay que especificar, además del nombre
que se le de a la articulación, el tipo de ésta, pudiendo ser:
ROS 87
Figura 3.13: Joints
fixed : articulación fija en la que no existe ningún grado de libertad.

Este tipo de articulación no requiere especificar las etiquetas axis, ca-
libration, dynamics, limits ni safety_controller.
revolute: articulación con un grado de libertad de revolución en torno

a un eje con un rango limitado de giro.
continuous: equivalente al tipo revolute pero con un rango de giro sin

límites.
prismatic: articulación de un grado de libertad de desplazamiento so-

bre un eje dentro de un rango limitado.
planar : articulación con 2 grados de libertad que permite el movimien-

to en un plano perpendicular al eje que se especifique.
floating: esta realmente no sería una articulación pues permitiría el

movimiento libre en los 6 grados de libertad.
Las etiquetas internas de cada joint tienen los siguientes significados y

argumentos:
<origin>: Transformación desde el sistema de referencia del link padre

al del link hijo. Como se comentó anteriormente, el joint se localizará
en el origen del sistema de referencia del link hijo, o más bien al revés,
el origen del link hijo (su sistema de referencia origen) se situará en el
del joint.
<origin xyz="0 0 1" rpy="0 0 3.1416"/>
<parent> y <child>: Links padre e hijo que une la articulación.

<parent link="link_1"/>
<child link="link_2"/>
<axis>: Eje rotación, de translación o perpendicular al plano de la ar-

ticulación especificado en el sistema de referencia de ésta. Por ejemplo,
en el eje z:
<axis xyz="0 0 1"/>
<dynamics>: Propiedades físicas de la articulación, permite especificar

unos valores de fricción (en N para las prismáticas y N m para las
de revolución) y de amortiguación (en N s m−1 para las prismáticas
y N m s rad−1 para las de revolución), siendo nulos los valores por
defectos.
<dynamics damping="3.0" friction="0.5"/>
<limit>: Limites superior e inferior del rango de movimiento (en m

para las prismáticas y rad para las de revolución), velocidad máxima
(en m/s o rad/s) y esfuerzo máximo aplicado (en N o N m) en la
articulación:
<limit lower="-2.2" upper="0.7"
velocity="1.0" effort="30" />
<safety_controller>: Permite especificar una serie de atributos rela-

cionados también con el truncamiento de las velocidades y esfuerzos
máximos:
<safety_controller soft_lower_limit="-2"
soft_upper_limit="3"
k_position="10" k_velocity="10" />
Esta última etiqueta merece de una explicación más amplia. soft_lower_limit

y soft_upper_limit especifican los límites superiores e inferiores que el con-
trolador de seguridad tendrá en cuenta, k_position especifica la relación de
proporcionalidad entre la velocidad y la posición cuando nos acercamos a
los límites anteriores y k_velocity la relación de proporcionalidad entre el
esfuerzo y la velocidad cuando nos acercamos a los límites en la velocidad
máxima17 . En la Figura 3.14 y en la Figura 3.15 se representan gráficamente
todas estas especificaciones.
Por otro lado, a cada joint es posible asociarle un actuador y una transmi-
sión entre ambos, que además serán necesarios para añadirle un controlador
como veremos en el paquete para controlar el manipulador. Para ello, la
descripción URDF posee la etiqueta <transmission>:
<transmission name ="Transmision_1" type ="SimpleTransmission">
<actuator name="actuador_1" />
<joint name="art_12" />
<mechanicalReduction>1</mechanicalReduction>
</transmission>
17
http://www.ros.org/wiki/pr2_controller_manager/safety_limits
ROS 89
Figura 3.14: Limites de seguridad en velocidad
Figura 3.15: Limites de seguridad en esfuerzo
Existen otras etiquetas para añadir ciertas propiedades y plugins espe-

cíficos de Gazebo (sensores de odometría, cámaras, etc.), pero se tratarán
en el bloque reservado para el simulador.
Por último, comentar dos utilidades más que presenta este stack y se
han usado en este proyecto:
check_urdf : Permite validar robots en formato URDF, devolviendo
si existe algún error en la descripción que le pasemos como argumento.
Para ello se ecribe una terminal algo similar a lo siguiente:
$ rosrun urdf check_urdf quadrotor.urdf
urdf_to_graphiz : Crea un diagrama gráfico de nuestro modelo URDF.

El ejecutable pertenece al paquete urdf_parser y se usaría de la si-
guiente forma:
$rosrun urdf_parser urdf_to_graphviz quadrotor.urdf
Common_msgs
Common_msgs es un stack que contiene una colección de mensajes que
son utilizados por un gran número de paquetes de ROS directamente o indi-
rectamente, es decir, para crear otro tipos de mensajes a partir de ellos. De
igual modo, cualquier usuario puede utilizarlos en sus propios programas y

crear otro tipo de mensajes que incluyan uno o varios de los tipos de men-
sajes contenidos en este stack. El uso de estos tipos de mensajes asegura la
interoperabilidad en cuanto a comunicaciones vía topic de los paquetes, por
lo cual es muy recomendable usar éstos o crear tipos de mensajes en base a
estos.
Para los programas que se crearán en este proyecto nos interesan aquellos
que están agrupados en el paquete geometry_msgs18 , que contiene men-
sajes para primitivas geométricas tales como puntos, vectores, posiciones,
orientaciones, velocidades y fuerzas. En concreto, se utilizan los siguientes
mensajes directamente y para crear otros mensajes:
Point.msg : Representación de la posición de un punto en el espacio.
Point:
float64 x
float64 y
float64 z
Vector3.msg : Representación de un vector en el espacio sin más in-

formación y es equivalente en cuanto a descripción al tipo de mensaje
anterior:
Vector3:
float64 x
float64 y
float64 z
Quaternion.msg : Representación de la orientación en el espacio en

forma de cuaternio:
Quaternion:
float64 x
float64 y
float64 z
float64 w
Pose.msg : Representación de la localización (posición y orientación)

en el espacio. Se compone de dos tipos de mensajes anteriores:
18
http://www.ros.org/wiki/geometry_msgs
ROS 91
Pose:
Point position
Quaternion orientation
Twist.msg : Representación de la velocidad lineal y angular en el es-

pacio.
Twist:
Vector3 linear
Vector3 angular
Wrench.msg : Representación de una fuerza generalizada (fuerzas y

momentos) en el espacio.
Wrench:
Vector3 force
Vector3 torque
PoseWithCovariance.msg : Contiene la representación de la loca-

lización, mediante el tipo de mensaje Pose, junto con la matriz de
covarianza asociada a ella.
PoseWithCovariance:
Pose pose
float64[36] covariance
TwistWithCovariance.msg : Contiene la representación de la ve-

locidad, mediante el tipo de mensaje Twist, junto con la matriz de
covarianza asociada a ella.
TwistWithCovariance:
Twist twist
WrenchStamped.msg : Contiene la representación de una fuerza ge-

neralizada, mediante el tipo de mensaje Wrench, junto con el tipo de
mensaje de encabezamiento Header del paquete std_msgs.
WrenchStamped:
Header header
Wrench wrench
Para poder utilizar estos tipos de mensajes, o crear otro tipo de men-
sajes en base a estos, es necesario incluir la dependencia a este paquete
(geometry_msgs) en el archivo manifest.xml del paquete donde se vaya
a crear el programa que lo utilize. Por ejemplo, para utilizar cualquiera de
los tipos anteriores es necesario añadir:
<depend package="geometry_msgs"/>
Gracias a esto será posible incluir el archivo de cabecera donde se define
el tipo de mensaje y crear variables del mismo tipo.
Esto ocurre en general con todos los paquetes que se vayan a utilizar en
un programa ejecutable en el sistema ROS, ya sea para tipos de mensajes,
tipos de servicios o cualquier función o clase que se quiera utilizar esté
definida en otro programa.
Roscpp - Time
En el apartado conceptos básicos ya se han comentado y ejemplificado
las clases y funciones que nos ofrece este paquete en cuanto a los nodos,
topics y servicios. Sin embargo, no se ha tratado todavía nada relaciona-
do con el acceso al tiempo o reloj real (wall time) o al tiempo de simulación.
Lo primero que ha de tenerse en cuenta es que si se está lanzando una

simulación en Gazebo, el tiempo será el que marque este simulador, un tiem-
po de simulación, que es el que ROS estaría enviando por el topic /clock
y al que se suscribirán automáticamente todos los nodos y accederán todas
las funciones que se tratan a continuación19 . Si por el contrario se está tra-
bajando con un robot real se accederá al tiempo o reloj real, pero este no
será nuestro caso.
Lo primero que ha de tenerse en cuenta es que si se está lanzando una si-

mulación en Gazebo el tiempo será el que marque este simulador, un tiempo
de simulación, que es el que ROS estaría enviando por el topic /clock y al
que se suscribirán automáticamente todos los nodos y accederán todas las
funciones que se tratan a continuación. Si por el contrario se está trabajando
con un robot real se accederá al tiempo o reloj real, que no será nuestro caso.
En cuanto variables de tiempo, ROS nos ofrece dos tipos de primitivas

(y sus clases respectivas de roscpp):
time: Tipo de primitiva para instantes de tiempo.

19
http://www.ros.org/wiki/Clock
ROS 93
duration: Tipo de primitiva para intervalos de tiempo.
Ambas poseen la misma estructura o variables miembros, dividiendo el

tiempo total en segundos y nanosegundos20 :
time/duration
int32 sec
int32 nsec
Sus clases asociadas en C++ (con iguales significados y estructuras) son

ros::Time y ros::Duration. Ambas poseen funciones miembros de gran
ayuda a la hora de controlar el flujo de un programa respecto al tiempo de
simulación:
Para obtener el tiempo actual de simulación:
ros::Time ahora = ros::Time::now();
Para obtener el instante en segundos o nanosegundos:
int segundos = ahora.toSec();
int nanosegundos = ahora.toNSec();
Para esperar a que comienze la simulación en Gazebo:
ros::Time::waitForValid();
Para pausar el programa durante un tiempo (de simulación) determi-

nado:
ros::Duration(3).sleep(); (Dormir 3 segundos)
ros::Duration(0,1000).sleep(); (Dormir 1000 milisegundos)
Control Toolbox
Control_toolbox es un paquete perteneciente a pr2_controller, un stack
que recopila diversos paquetes para el control del robot PR2 pero que pue-
den ser utilizados, en parte, para otros robots. Entre estos se encuentra
control_toolbox, que contiene una serie de clases C++ útiles para imple-
mentar controladores de una manera sencilla y para cualquier robot21 .
20
http://www.ros.org/wiki/roscpp/Overview/Time
21
http://www.ros.org/wiki/control_toolbox
En concreto, en este proyecto se utilizará la clase Pid 22 , que proporciona

la estructura de un bucle de control proporcional-integral-derivativo están-
dar, con la salvedad de que la actuación estaría cambiada de signo respecto
al estándar:
u = −(P + D + I)
Esto se debe a que, en esta clase, se supone que el error viene definido
también cambiado de signo respecto a su forma estandar:
e = V alor actual − V alor deseado
Los términos del controlador son los siguientes:
Término proporcional (P): Producto del error por el coeficiente de
proporcionalidad (p):
P = p ⋅ e(k)
Término integral (I): Producto de la ganancia integral (i), que equivale

al inverso del tiempo integral, por la aproximación rectangular hacia
atrás de la integral del error:
I = i ⋅ int_e(k)
int_e(k) = int_e(k − 1) + e(k) ⋅ dt
Término derivativo (D): Producto del tiempo derivativo (d) por la

aproximación de la derivada del error en ese instante:
D = d ⋅ e(k)−e(k−1)
dt
En cuanto al incremento o diferencial de tiempo, dt, debe tenerse en

cuenta que se trata del de simulación en nuestro caso.
Además de los coeficientes del controlador p, i y d, es posible definir unos

límites sobre el término integral (anti-windup) mediante la variable interna
de la clase i_clamp.
Los coeficientes p, i, d e i_clamp pueden definirse directamente en el

programa y crease el controlador mediante el constructor de la clase fácil-
mente. Sin embargo existe otro opción, cargarlos como parámetros desde el
SP de ROS. Esta última opción es la que se utilizará para los controladores
del manipulador y, para ello, se usará otro de los constructores de la clase
al que se le debe especificar donde están están definidos estos parámetros
(p, i, d e i_clamp) en el servidor:
22
http://www.ros.org/doc/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.
html
ROS 95
//Creación objeto de la clase Pid

control_toolbox::Pid pid_1;
//Inialización con los datos del SP

pid_1.init(ros::NodeHandle(n, "pid_1_parametros"))
Para la última instrucción han de tenerse cargados con antelación estos

4 parámetros, en el SP, dentro del grupo "pid_1_parámetros", en este caso,
lo cual puede hacerse desde el lanzador del mismo sistema de control. Esto
último requiere de otros pasos que se explicarán en el capitulo de control
desacoplado del manipulador.
La ventaja de este tipo de inicialización es que es posible modificar los

coeficientes del controlador sin tener que volver a compilar el programa, lo
cual ahorra bastante tiempo a la hora de sintonizar el PID.
Una vez creado el controlador, podemos utilizar la función miembro up-

datePid() que tiene la siguiente definición:
double control_toolbox::Pid::updatePid( double error, ros::

Duration
dt )
Donde los argumentos de entrada serían el error (en posición) y el dife-

rencial de tiempo, y nos devolvería la actuación (momento a aplicar) para
ese instante.
Esta clase posee muchas más funciones miembro que no se van a tratar
aquí al no utilizarse para los controladores de nuestras robots. Pueden verse
todas ellas en la web referenciada en la segunda nota al pie de página, dos
páginas atrás.
Pr2_mechanism
pr2_mechanism es un stack destinado a ofrecer una infraestructura para

el control de cualquier robot mediante un ciclo de control en tiempo real.
En principio fue creada para el control exclusivo del robot PR2, de ahí su
nombre, pero desde la web de ROS se asegura que debe funcionar en cual-
quier robot. De hecho es utilizada para el control, entre otros, de los brazos
manipuladores de este robot, por lo que en principio debe servir para el
control del manipulador de este proyecto.
El stack se compone de una serie de paquetes para dar soporte tanto a

robots reales como simulados y ofrecer una interfaz de programación sencilla
en C++ para nuestros propios controladores. A continuación se presentarán
los paquetes que nos interesan y que se han utilizado, obviando aquellos que
van destinados únicamente al control de robots reales.
1. pr2_controller_interface: Ofrece la interfaz C++ para especifi-

car el controlador en tiempo real. Contiene definida una clase base,
pr2_controller_interface::Controller, de forma que toda clase
que herede la forma de ésta puede ser ejecutada por el nodo Con-
troller Manager en un ciclo del control en tiempo real.
La clase base Controller tiene la siguiente definición:
namespace pr2_controller_interface
{
class Controller
{
public:
virtual bool init(pr2_mechanism_model::
RobotState *robot,
ros::NodeHandle &n);
virtual void starting();
virtual void update();
virtual void stopping();
bool getController(const std::string& name,
int sched,
ControllerType*& c);
};
}
Para crear nuestro controlador, basta con crear una clase que here-
de esta forma (sea una clase hija de la clase Controller ) y modificar
la definición de sus funciones miembros init(), starting(), update() y
stopping() según lo que deseemos que haga nuestro controlador.
Figura 3.16: Funciones miembro clase Controller
La función init() es la primera en ser llamada por Controller Manager,

recibiendo un objeto pr2_mechanism_model::RobotState, que contiene
nuestro modelo URDF del robot y da acceso a cada una de las partes
de éste. También se recibe el objeto ros::NodeHandle asignado para
tener acceso al Servidor de parámetros y poder crear y subscribirse a
topics y servicios. Por tanto, está es la función donde deben obtenerse
todos los datos que nos hagan falta del SP y de nuestro modelo URDF,
así como crear las variables y objetos que necesitemos a partir de ellos.
ROS 97
La función starting() es llamada una sola vez por Controller Manager

al inicio del primer ciclo de control y en ella deben inicializarse todas
las variables que necesitemos, como las posiciones actuales y deseadas
iniciales y el instante de tiempo actual.
La función update() es la función principal del controlador y la que

será ejecutada cada ciclo de control en tiempo real a 1000 Hz. Control
Manager es el que se encargará de esta ejecución, emitiendo un mensa-
je de error en caso de que se sobrepase el milisegundo en esta función.
En ella debe de programarse todo el sistema de control, es decir, los
cálculos y acciones de control pertinentes.
La función stopping() es ejecutada una sola vez al finalizar el control.

En ella se programarían acciones de liberación de memoria o actuali-
zación de datos finales.
La última función miembro de la clase carece de importancia para este

proyecto.
2. pr2_controller_manager : Contiene la infraestructura para cargar,

inicializar, ejecutar y parar en tiempo real múltiples controladores en
el sistema ROS. Como ya se ha comentado, también es el que se en-
cargará de comprobar que la ejecución sea a 1000 HZ y en tiempo
real, emitiendo un mensaje de error en caso de que no sea así. Real-
mente el Controller Manager puede entenderse como un nodo más
del sistema que se encargará de llevar a acabo el programa definido
en nuestra clase controlador anterior, ejecutando la función update()
y aplicando las fuerzas que se especifiquen sobre el robot. Para ello,
pr2_controller_manager ofrece el nodo spawner que permite cargar
y ejecutar los controladores que se le pasen como argumento.
3. pr2_controller_model : Contiene la definición, entre otras, de tres

clases muy útiles para acceder al robot (más concretamente a sus ar-
ticulaciones) durante la simulación:
pr2_mechanism_model::RobotState: clase que contiene el

modelo del robot en formato URDF así como una función, get-
JointState(), para obtener, a partir del nombre de la articulación,
objetos de la clase pr2_mechanism_model::JointState:
pr2_mechanism_model::JointState: clase que da acceso en
tiempo de simulación a una articulación en concreto, permitiendo
mediante diversas funciones miembros obtener la posición (po-
sition_), la velocidad (velocity_) y el esfuerzo actual (measu-
red_effort_), así como enviar un esfuerzo deseado (commanded_effort_)
a la articulación (en nuestro caso al joint correspondiente en Ga-
zebo).
Para entender las clases anteriores y presentar cada función co-

mentada se presenta el siguiente ejemplo:
// Suponiendo obtenido pr2_mechanism_model::
RobotState
// *robot, de la función init()
// Creamos un Objeto JointState

pr2_mechanism_model::JointState* articulacion_1;
// Obtenemos acceso a la articulación declarada
// como "articulacion_1" en el URDF mediante el
// objeto Robot State
articulacion_1 = robot->getJointState("
articulacion_1")
// Obtenemos la posición actual de "articulacion_1
"
double posicion = articulacion_1->position_;
// Obtenemos la velocidad actual de "
articulacion_1"
double velocidad = articulacion_1->velocity_;
// Obtenemos el esfuerzo actual sobre
// "articulacion_1"
double M1_actual = articulacion_1->
measured_effort_;
// Establecemos el esfuerzo a aplicar sobre
// "articulacion_1"
articulacion_1->commanded_effort_ = 10;
Ambas poseen más funciones pero no serán utilizadas en este pro-

yecto.
pr2_mechanism_model::Chain: Clase para trabajar con ca-
denas cinemáticas de varios joints. Se obtiene a partir del objeto
RobotState y los nombres de los link inicial y final de la cadena:
// Objeto del tipo Chain

pr2_mechanism_model::Chain cadena;
// Cadena desde "Conjunto_base" a "Chasis_efector"
cadena.init(robot_state, "Conjunto_base", "
Chasis_efector");
Contiene multitud de funciones equivalentes a las de JointState

pero que, en este caso, serían referidas a conjuntos de articu-
laciones. Para estas finalidades se usará la clase JointState. Sin
embargo si será necesario utilizar la clase Chain para obtener otra
clase tipo cadena de joints, equivalente a ésta, pero para el paque-
te KDL (que se verá a continuación). Para ello se usa la función
Chain::toKDL(KDL::Chain cadena_kdl), que nos guardará
en el argumento que se le pasa la misma cadena pero con el tipo
ROS 99
de clase KDL::Chain.
Como siempre, este stack contiene más paquetes con tipos de mensajes
y herramientas de líneas de comandos que no han sido utilizados para nues-
tras aplicaciones, por lo que se obvian sus explicaciones para no cargar de
tanto contenido teórico el mismo.
Para utilizar esta infraestructura de control, es necesario crear y definir

una serie de programas, archivos de configuración y de lanzamiento dentro
de un paquete de ROS destinado al control de robot en particular. A con-
tinuación se comentan dichos programas y archivos necesarios en la forma
en que se han realizado en este proyecto:
Archivo de cabecera: control.h. En él se declara la nueva clase, hija de

la clase Controller, sobreescribiendo las funciones miembro de esta y
definiendo las variables privadas y/o publicas que se vayan a utilizar
si se desea.
Programa o archivo de implementación: control.cpp. En él se definen
las funciones miembros de nuestra clase de control definida en el ar-
chivo de cabecera, y es el que será ejecutado por Controller Manager.
Librería: control_manipulador_lib.so. En este caso no se tendrá un
ejecutable del programa directamente, sino que se crea una librería a
partir del programa de implementación que será ejecutada por Con-
troller Manager. Para ello se ha de añadir la siguiente instrucción en
el archivo CMakeLists.txt del paquete:
rosbuild_add_library(control_manipulador_lib src/control.cpp)
Plugin para la librería: control_manipulador_plugin.xml. Para que

Controller Manager tenga acceso a esta librería y pueda ejecutarla
es necesario crear un plugin hacia ella mediante este archivo, el cual
tiene la siguiente forma:
<library path="lib/libcontrol_manipulador_lib">
<class
name="control_manipulador/ControlManipuladorPlugin"
type="control_nspace::ClaseControl"
base_class_type="pr2_controller_interface::Controller"
/>
</library>
Para crearlo se añade en control.cpp el siguiente macro, en el que se

especifica el nombre del paquete, del plugin, de la nueva clase control
y de su clase padre:
PLUGINLIB_DECLARE_CLASS(
control_manipulador,
ControlManipuladorPlugin,
control_nspace::ClaseControl,
pr2_controller_interface::Controller)
Archivo de parámetros: controladores.yaml. En este archivo se guar-

dan todos los parámetros que vayamos a necesitar para el controlador
y que se cargarán y leerán desde el Servidor de Parámetros: nombre
de las articulaciones del robot a controlar (los mismos que tengan en
el modelo URDF) y valores de los coeficientes de los PIDs. Estos son
opcionales, ya que se usan en control.cpp y, sabiéndolos a priori, po-
dríamos definirlos ahí directamente. El único parámetro obligatorio a
definir en éste archivo y cargar en el SP es el plugin creado anterior-
mente bajo el parámetro type.
Archivo de lanzamiento: controladores.launch. En él se carga el archi-
vo de parámetros anterior y se ejecuta el controlador mediante el no-
do spawner de pr2_controller_manager pasándole como argumentos
el archivo de parámetros (necesario para que ejecute el controlador-
librería a partir del plugin).
Todos estos archivos se crearán para cada uno de los controladores del
manipulador realizados en este proyecto.
Orocos Kinematics and Dynamics

ROS contiene un stack, denominado con este nombre, en el que se im-
plementa la librería KDL (Kinematics and Dynamics Library) del proyecto
Orocos[12], una aplicación para modelar y realizar cómputos de cadenas ci-
nemáticas. Contiene definidas una serie de clases y funciones que permiten
crear cadenas formadas por eslabones y articulaciones, así como resolver
para ellas los modelos cinemáticos directo e inverso, entre otros.
El stack se encuentra dividido en tres paquetes:

kdl: Contiene la versión más reciente de la librería KDL.
orocos_kdl: Versión en C++ de ésta librería.
python_orocos_kdl: Versión en python de la misma.
Como es de esperar, para este proyecto el que nos interesa es el paquete
orocos_kdl, para utilizar sus tipos de variables, clases y funciones desde
nuestro programa en C++.
Dentro de este paquete se encuentran definidos un gran número de tipos

de variables, clases y funciones, de las cuales solo se utilizarán un reducido
número para aplicarlas al control cinemático del brazo manipulador, como
se verá en el control de éste, por lo que se explicarán a continuación solo
aquellas que se han utilizado. 23 .
23
Si se desea más documentación sobre esta librería puede visitar la web de la misma: http:
//www.orocos.org/kdl
ROS 101
Pasando directamente a las variables y clases básicas utilizadas de esta

librería, tenemos las siguientes:
KDL::Vector : Clase que contiene un vector de 3 dimensiones. Su construtor[13]

tiene la siguiente definición: KDL::Vector p(double x, double y, double
z), y para acceder, por ejemplo, al primer basta con escribir p(0).
KDL::Rotation: Clase que contiene una matriz de rotación en 3 di-
mensiones. Su constructor y acceso a los elementos es equivalente al
de la clase anterior: KDL::Rotation M(double Xx, double Yx, double
Zx, ..., double Zz) y M(0,2).
KDL::Frame: Clase que contiene una matiz de transformación homo-
genea tridimensional, formada por un vector y una matriz de rotación
de las clases anteriores.
KDL::JntArray: Clase que implementa un vector de dimensión varia-
ble igual al número de las articulaciones de la cadena cinemática. Se
puede utilizar para posiciones, velocidades, aceleraciones y momen-
tos. Su constructor y acceso a las variables es equivalente que para
KDL::Vector. La función miembro con la que se le proporciona a la cla-
se el tamaño del vector (el número de joints) es KDL::JntArray::resize(int
n_joints).
KDL::Chain: Clase que contiene la descripción completa de una cade-
na cinemática de forma equivalente a pr2_mechanism_model::Chain.
Su utilidad radica en servir como argumento para las clases solvers
para la resolución de la cinemática directa e inversa, que se verán
a continuación. Su constructor no nos interesa ya que, como se co-
mento en el stack anterior, se obtendrá a partir del objeto cadena de
pr2_mechanism_model. Entre sus funciones miembros se encuentran
dos de gran utilidad: getNrOfJoints() y getNrOfSegments(), para la
obtención del número de articulaciones y del número de eslabones,
respectivamente, que forman parte de la cadena.
Las funciones para la resolución de la cinemática directa e inversa de

una cadena, se encuentran encapsuladas dentro de unas clases que imple-
mentan estas como funciones miembro, y de las cuales han de comentarse
las siguientes:
KDL::ChainFkSolverPos_recursive: Clase que encapsula un método

para la resolución de la cinemática directa en posición denominado
JntT oCart(KDL ∶∶ JntArray &Q, KDL ∶∶ F rame &X) (coordenadas
articulares Q a cartesianas X). Para construir un objeto de esta clase es
necesario pasarle como argumento la cadena cinemática (KDL::Chain)
en cuestión.
KDL::ChainIkSolverPos_NR: Clase que encapsula un método para la
resolución de la cinemática inversa en posición denominado CartT oJnt(
KDL ∶∶ JntArray &Q_inicial, KDL ∶∶ F rame &X, KDL ∶∶ JntArray
&Q) (coordenadas cartesianas X a articulares Q, dada una aproxima-

ción Q_inicial) utilizando el método de Newton-Raphson. Para cons-
truir un objeto de esta clase es necesario pasarle como argumento la
cadena cinemática en cuestión, un solver de la CD como el anterior
y un solver de la CI en velocidad (el siguiente solver que se explica)
para ser utilizado internamente en el método de Newton-Raphson.
KDL::ChainIkSolverVel_pinv : Clase que encapsula un método para

la resolución de la cinemática inversa en velocidad, con la misma de-
claración que la del solver anterior. Para construir un objeto de esta
clase solo es necesario pasarle como argumento la cadena cinemática
en cuestión.
Consideresé el siguiente ejemplo para aclarar estos conceptos a la hora

de inspeccionar el código realizado para el controlador cartesiano de nuestro
brazo manipulador:
// Reservamos memoria para un objeto de la clase
// KDL::ChainFkSolverPos, que no es mas que la clase
// padre de KDL::ChainFkSolverPos_recursive de igual
// definicion
boost::scoped_ptr<KDL::ChainFkSolverPos> solver_CD;
// Construimos nuestro solver en la memoria reservada

// y para un objeto KDL::Chain previamente obtenido
solver_CD.reset(new KDL::ChainFkSolverPos_recursive(cadena_kdl
));
// Resolvemos la cinematica directa, dados un KDL::JntArray

// del tamaño del numero de articulaciones en la cadena, q, y
// obteniendo un KDL::Frame, x, con la localizacion del
// extremo de esta
solver_CD->JntToCart(q, x);
3.1.4 Herramientas comunes

Vamos a ver ahora las herramientas que nos ofrece ROS para la visua-
lización, análisis y registro de datos de una aplicación (tanto en un robot
real como simulado). Básicamente se encuentran todas en las pilas visua-
lization y rx.
Todas las herramientas que se comentan a continuación siguen siendo

nodos en el sistema, con sus topics y servicios, y se pueden ejecutar tanto
desde una terminal directamente como desde un archivo launch, como se
hará en general para todas las simulaciones24 .
24
http://www.ros.org/wiki/Tools
ROS 103
Visualización modelos - rviz
rviz es un entorno de visualización 3D que permite combinar en una

misma pantalla modelos de robots, datos de sensores (cámara, láser, . . . )
y otros datos en 3D. También es posible crear marcadores enviándolos di-
rectamente desde nuestro programa. Esta herramienta se encuentra en el
paquete rviz del stack visualization, y su ejecución desde una terminal es
como para cualquier otro nodo:
$ rosrun rviz rviz
En este proyecto se usará para hacer comprobaciones sobre la configu-

ración y dimensiones de los robots (quadrotor y manipulador), así como
para calcular los límites físicos de las articulaciones y comprobar los límites
impuestos.
Para lo primero basta con ejecutar rviz y cargar el URDF del robot en
el Servidor de parámetros con el nombre robot_description. Una vez hecho
esto se puede se agregar el display denominado Robot model que permite
visualizar el modelo del robot cargado en el servidor tal y como se muestra
en la Figura 3.17.
Figura 3.17: Modelo en rviz
Es posible visualizar a partir de este display tanto el modelo visual co-

mo el de colisión, los sistemas de coordenadas de cada cuerpo o eslabón
del robot, todo a partir de la información contenida en el archivo URDF.
Además podemos variar el número de grids, así como su posición, el número
de divisiones y el tamaño al que deseemos, lo cual es bastante útil para
comprobar las dimensiones del robot.
Para poder comprobar los movimientos y límites de las articulaciones,

tal y como se muestra en la Figura 3.18, del modelo descrito en formato
URDF es necesario hacer uso de otros dos programas:
robot_state_publisher : Publica directamente los sistemas de coor-

denadas de cada elemento del robot para que sean accesibles por cual-
quier programa, y en particular por el siguiente.
joint_state_publisher : Permite modificar el estado de las articula-
ciones, de sus SR asociados, mediante una interfaz gráfica (GUI ).
Figura 3.18: rviz y joint_state_publisher
Para no tener que estar ejecutando varios programas y cargando dife-

rentes parámetros cada vez que queramos realizar visualizaciones en rviz de
nuestros robots, crearemos unos lanzadores (.launch) que carguen el modelo
deseado en el servidor, ejecuten rviz y carguen la configuración de rviz con
los displays oportunos. En cuanto a los códigos, se tratarán más adelante
para nuestras propias aplicaciones para el quadrotor y para el manipulador.
Gráficas - rxplot
rxplot es una herramienta perteneciente al paquete rxtools del stack rx
que sirve para obtener gráficos en línea a partir de uno o varios topics.
Solo es posible, como es lógico, sacar gráficas de campos concretos de

los topics, es decir, de valores escalares. Por ejemplo, si tenemos un topic
por el que se está enviando la posición de un robot, podemos obtener una
gráfica en línea como la que se muestra en la Figura 3.19.
rxplot, al igual que rviz, sigue siendo un nodo y por tanto se puede
ejecutar desde una terminal o desde un archivo launch pasándole como
argumento los campos del topic o de los topic de los que se quieren obtener
gráficas. El orden de los argumentos es el que determina la organización de
las gráficas. Por ejemplo, para sacar dos campos en gráficas diferentes, como
las de la Figura 3.19:
$ rxplot /topic1/field1 /topic2/field2 25
Para que se representen en la misma gráfica sería igual pero separadas

por una coma:
ROS 105
Figura 3.19: rxplot
$ rxplot /topic1/field1 /topic2/field2
Grafos - rxgraph
Con rxgraph se obtiene un gráfico de todos los procesos que se están eje-
cutando en ROS junto con sus conexiones, es decir, un grafo con los nodos
(elipses) y los topics (líneas) en ejecución.
Figura 3.20: rxgraph
El grafo mostrado en la Figura 3.20, es el correspondiente a una de

las simulaciones que se llevarán a cabo en este proyecto para el quadrotor,
en el que se observa como se están ejecutando diferentes nodos (/control,
/cámara, . . . ) y se están enviando mensajes entre ellos mediante topics (/O-
dometría, /Giro_Motores,. . . ).
Esta herramienta es de gran utilidad para saber que nodos están en eje-
cución y que relación tienen entre ellos, lo cual sirve también para ver si ha
fallado la ejecución de algún nodo o finalizado su ejecución. Aún así, tiene
el inconveniente de que no representa los servicios, por lo que este proyecto
carecerá en ocasiones de gráficos para ellos.
Se encuentra en el paquete rxgraph del stack rx, y como todos los ante-
riores sigue siendo un nodo más, aunque no aparecerá en su propio grafo y,
al igual que rxplot, está integrado en el sistema como herramienta de línea
de comandos, por lo que para ejecutarlo simplemente se haría desde una

terminal:
$ rxgraph
Registro por consola - rxconsole
rxconsole es un visualizador de mensajes que están siendo publicados por

/rosout. Pertenece también al paquete rxtools del stack rx y está integrado
en el sistema como herramienta de línea de comandos.
Figura 3.21: rxconsole
Como se ve en la Figura 3.21, rxconsole nos muestra los mensajes según

el tipo o nivel de alarma, mostrándonos el contenido de cada mensaje, el
nodo que lo publica y el instante de publicación entre otros. Además per-
mite seleccionar el tipo de los mensajes que queremos que nos muestre y los
que no26 .
Los tipos de mensajes que aparecen son aquellos que se estén enviando
desde los nodos mediante unas instrucciones del tipo:
ROS_INFO("Valor de x: [ %f ]",x);
Existen 5 tipos fundamentales de mensajes cuya sintaxis es la misma

que la del ejemplo y que, en orden de importancia, son:
DEBUG, para información transparente hacia el usuario.
INFO, para información que pueda resultar útil al usuario.
WARN, para fallos posibles, esperados y que tienen solución.
ERROR, errores serios pero recuperables.
FATAL, errores no recuperables.

26
http://www.ros.org/wiki/rxconsole
GAZEBO 107
3.2 Gazebo
3.2.1 Introducción a Gazebo
Gazebo, que nace con el nombre de Gazebo Project, es un simulador 3D,
cinemático, dinámico y multi-robot que permite realizar simulaciones de ro-
bots articulados en entornos complejos, interiores o exteriores, realistas y
tridimensionales[14].
Entre sus características podemos nombrar las siguientes:

Gazebo es un software libre financiado, en parte, por Willow Garage,
pudiendo ser reconfigurado, ampliado y modificado
Compatible con ROS y Player. Podemos ejecutar Gazebo desde ROS
y utilizar las APIs de este último para controlar los robots en las
simulaciones, es decir, enviar y recibir datos de éstas.
Simulación realista de la física de los cuerpos rígidos. Los robots pue-
den interactuar con el mundo (pueden coger y empujar cosas, rodar
y deslizarse por el suelo) y viceversa (les afecta la gravedad y pueden
colisionar con obstáculos del mundo).
Capacidad de desarrollar y simular modelos de robots propios (URDF)
e incluso cargarlos en tiempo de ejecución.
Posibilidad de crear escenarios (mundos) de simulación, variando las
características de los contactos con el suelo, los obstáculos e incluso
los valores de la gravedad en las tres dimensiones. También es posible
variar las características de contacto de cada link individualmente.
Contiene diversos plugins para añadir sensores al modelo del robot y
simularlos, como sensores de odometría (GPS e IMU), de fuerza, de
contacto, lásers y cámaras estéreo.
Internamente, Gazebo utiliza:
Open Dynamics Engine (ODE), un motor de física basada en la formu-
lación de problemas sobre la complementariedad de restricción (LCP).
Se compone de una serie de bibliotecas de alto rendimiento para la
simulación de la cinemática y de la dinámica de cuerpos rígidos.
OGRE, motor de renderizado de escenas gráficas en 3D.
En cuanto a su integración en ROS, el simulador posee su propio stack
denominado simulator_gazebo 27 , que se divide en una serie de paquetes
con diferentes funcionalidades:
gazebo: Contiene la versión más reciente de Gazebo Project integra-
da en ROS, de forma que es ejecutable como un nodo más de ROS,
poseyendo sus propios topics y servicios).
27
http://www.ros.org/wiki/simulator_gazebo
gazebo_msgs: Contiene los tipos de servicios (srv) y mensajes (msg)

para interactuar con Gazebo desde ROS. Por tanto los tipos de men-
sajes y servicios a utilizar para enviar o recibir del simulador deben
ser incluidos a partir de este paquete.
gazebo_plugins: Contiene los plugins para utilizar los diferentes sen-
sores.
gazebo_tools: Contiene, entre otras herramientas, la que nos permiten
enviar o eliminar modelos URDF en el simulador, mediante el nodo
gazebo_model, del mismo paquete28 .
gazebo_worlds: Contiene los archivos de configuración estándares del
entorno (.world) desarrollados por Willow Garage, así como diferen-
tes modelos URDF de objetos (muros, mesas, sillas, . . . ) a modo de
ejemplo.
3.2.2 Configuración y generalidades del entorno de simulación

Para ejecutar el simulador con un entorno ya configurado es necesario
pasarle como argumento un archivo de configuración de extensión .world.
Este tipo de archivo se escribe en XML y en él se especifican parámetros del
motor físico ODE (como el tiempo de integración, el valor de la gravedad,
. . . ), del motor gráfico OGRE (renderizado, sombras, cielo, ambiente, . . . )
y de los objetos que deben aparecer en el simulador automáticamente.
En las simulaciones llevadas a cabo en este proyecto todas las simulacio-

nes se han realizado usando la configuración por defecto de Willow Garage
de su archivo de configuración empty.world del paquete gazebo_worlds.
Por tanto, en todos los lanzadores de nuestras simulaciones aparecerá lo
siguiente:
<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find
gazebo_worlds)/worlds/empty.world" respawn="false" output="log"/>
Esta instrucción ejecuta el nodo del simulador cargando la configuración
dicha anteriormente29 . Aunque no se vaya a modificar, cabe comentar al-
gunas de las cosas que se están configurando para tenerse en cuenta a la
hora de posibles análisis en nuestras simulaciones. Los parámetros de ODE
serían los siguientes:
<physics:ode>
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
28
Existe otra versión de este nodo denominada spawn_model con la misma utilidad pero que
esta definido en el paquete gazebo
29
Además de pasarle la configuración del simulador, también se le está especificando que el simu-
lador comience en modo pausa.
GAZEBO 109
<quickStepIters>10</quickStepIters>
<quickStepW>1.3</quickStepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>
Donde stepTime es el paso de integración del simulador, gravity es el vec-
tor de gravedad aplicado sobre cada cuerpo, cfm (constraint force mixing)
es un parámetro entre 0 y 1 que especifica la dureza de las restricciones de
los robots en cuanto a los contactos, de modo que mientras menor sea más
duras serán las restricciones y mientras mayor sea más blandas (permitien-
do cierta penetración entre dos cuerpos) y erp (error reduction parameter )
es otro parámetro entre 0 y 1 para reducir el posible error en las articulacio-
nes que, al integrar, haría que no se cumplan exactamente las restricciones
impuestas sobre ellas, debido a las posiciones/orientaciones de los cuerpos
que une. Las etiquetas en las que aparece quickStepIterestán relacionadas
con un nuevo método de integración, en el que no entraremos en este pro-
yecto, pero que agiliza la simulación. Por último, contactMaxCorrectingVel
que define la máxima velocidad entre contactos (por defecto sería infinita) y
contactSurfaceLayer que define una profundidad permitida de penetración
antes de estabilizarse un robot en el suelo, y que en caso de ser exactamente
cero puede no llegarse a esta estabilidad.
Por otro lado, la configuración del suelo propiamente dicho:

<model:physical name="gplane">
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
<static>true</static>
<body:plane name="plane">
<geom:plane name="plane">
<laserRetro>2000.0</laserRetro>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000000.0</kp>
<kd>1.0</kd>
<normal>0 0 1</normal>
<size>51.3 51.3</size>
<segments>10 10</segments>
<uvTile>100 100</uvTile>
<material>Gazebo/GrayGrid</material>
</geom:plane>
</body:plane>
</model:physical>
Donde se están especificando la localización, restricción de movimiento
(static), tamaño, textura, reflexión de la luz y normal al plano en cuanto
a la fricción, entre otros. Caben destacar cuatro parámetros que permiten
simular diferentes contactos entre las superficies de los cuerpos de los robots
con el suelo y, como se verá, también entre ellos, ya que se les pueden
especificar estos mismos parámetros a los links de nuestros modelos URDF

junto a los plugins de Gazebo:
kp: Rigidez de contacto.

kd : Amortiguación de contacto.
mu1 : Fricción en dirección principal x de la superficie.
mu2 : Fricción en dirección principal y de la superficie.
Estos parámetros son los que permiten simular diferentes contactos y

son los que se han usado para el gancho del manipulador, definiéndolos en
referencia a Gazebo en el modelo URDF.
El resultado final que se muestra en la figura Figura 3.22 es el simulador

Gazebo, de momento sin ningún robot más que el propio suelo, ejecutándose
para la configuración de empty.world.
Figura 3.22: Gazebo empty.world
Una vez en marcha Gazebo, es posible cargar el modelo de cualquier

robot en cualquier momento y en cualquier posición mediante el ejecutable
spawn_model desde una terminal, desde un launcher o incluso desde un
programa en C++. Por lo general los robots que se vayan a usar en la simu-
lación se suelen cargar en el mismo launcher que ejecuta el simulador. Para
ello, es necesario primero leer el modelo URDF del robot y guardarlo en un
parámetro30 , por ejemplo una cadena denominada robot_descriptión. Así,
las dos instrucciones a añadir en el lanzador para mandar nuestro modelo
a Gazebo serán del tipo:
<param name="robot_description" command="$(find xacro)/xacro.py
$(find modelquad)/robots/table.urdf.xacro" />
<node name="spawn_man_mesa" pkg="gazebo" type="spawn_model"

args="-urdf -param robot_description -x 0 -y 0 -z 0.41 -model
table_model" respawn="false" output="screen" />
30
En este caso también se le está pasando el nodo xacro.py, dado que el modelo URDF contiene
unas simplificaciones de descripción que ofrece este nodo.
GAZEBO 111
Como puede observarse, hay una serie de argumentos para la ejecución

de spawn_model. Simplemente se le está indicando el tipo de archivo del
que se trata (-urdf ), el parámetro del servidor donde está almacenada su
descripción, la posición inicial donde debe aparecer en el simulador y el
nombre con el que debe aparecer (-model ), ya que podríamos cargar varias
veces este modelo en el simulador.
Figura 3.23: Modelo simple en Gazebo
Respecto a la interfaz gráfica, Gazebo nos ofrece la posibilidad de pausar

y reiniciar la simulación cuando se quiera, manipular directamente todos los
objetos y robots que tengamos cargados en él (Figura 3.24), e inspeccionar
los sistemas de referencia inerciales, tal y como se vio en la Figura 2.54.
Figura 3.24: Manipulación directa de objetos
Esto último permite modificar las posiciones iniciales o durante la eje-

cución directamente desde la interfaz del simulador estando éste pausado o
en marcha. Puede utilizarse esto para meter perturbaciones en posición a
nuestros modelos y jugar a ver cuan buenos son nuestros controladores.
Por otro lado, Gazebo puede usar el reloj de simulación de ROS que se
publica como topic /clock , del que ya se hablado anteriormente, y al cual
se suscriben automáticamente los demás nodos que se ejecuten de forma que
compartan el mismo vector de tiempo. Gazebo es el que dirigirá el tiempo
de simulación, de tal forma que cuando la simulación se para, por lo que
también se para el reloj de la simulación, todos los demás nodos que se
estén ejecutando y usen éste reloj también se paran. Además, ha de tenerse
en cuenta que la velocidad de la simulación puede variar según la carga de

cálculos que supongan, por lo que Gazebo también variará la velocidad del
reloj de simulación para que todos los demás nodos lo tengan en cuenta.
Para que Gazebo use el tiempo de simulación ha de especificarse aña-

diendo al launcher el siguiente parámetro, que lo leerá automáticamente del
servidor:
<param name="/use_sim_time" value="true" />
Los mensajes de tiempo que Gazebo envía por /clock tienen la misma
estructura que cualquier otro mensaje de tiempo: [sec, nsec], con el mismo
significado que para la clase ros::Time.
3.2.3 Topics y servicios de Gazebo
Gazebo, como nodo que es, publicará y se suscribirá a una serie de topics
al igual que ofrecerá al sistema una serie de servicios.
Los topics que ofrece y los topics a los que se suscribe por defecto son
los mostrados en el grafo de la Figura 3.25.
Figura 3.25: Topics y servicios de Gazebo
Como puede intuirse por sus nombres, todos están relacionados con el
estado (posiciones, parámetros y descripción) de los objetos existentes en el
simulador. Dado que usaremos los topics propios de los plugins, que se ve-
rán a continuación, para obtener este tipo de información, no se describirán
estos topics por defecto al no utilizarse en las aplicaciones de éste proyecto,
a excepción del ya comentado /clock.
Sin embargo, sí se usarán algunos de los servicios que Gazebo ofrece por
defecto, siendo el listado completo de servicios que ofrece el siguiente:
GAZEBO 113
/gazebo/apply_body_wrench
/gazebo/apply_joint_effort
/gazebo/clear_body_wrenches
/gazebo/clear_joint_forces
/gazebo/delete_model
/gazebo/get_joint_properties
/gazebo/get_link_properties
/gazebo/get_link_state
/gazebo/get_loggers
/gazebo/get_model_properties
/gazebo/get_model_state
/gazebo/get_physics_properties
/gazebo/get_world_properties
/gazebo/pause_physics
/gazebo/reset_simulation
/gazebo/reset_world
/gazebo/set_joint_properties
/gazebo/set_link_properties
/gazebo/set_link_state
/gazebo/set_logger_level
/gazebo/set_model_configuration
/gazebo/set_model_state
/gazebo/set_parameters
/gazebo/set_physics_properties
/gazebo/spawn_gazebo_model
/gazebo/spawn_urdf_model
/gazebo/unpause_physics
En concreto se usarán cuatro de los servicios que ofrece Gazebo, que se

explicarán a continuación junto con sus tipos de mensajes:
1. /gazebo/apply_body_wrench: Permite aplicar fuerzas y momen-
tos a un cuerpo en simulación. El tipo de mensaje de este servicio es
ApplyBodyWrench.srv, el cual se muestra a continuación en su versión
extendida (con los componentes de cada miembro del mensaje debajo
de cada uno y tabulados):
string body_name //Cuerpo sobre el que se ejercen

string reference_frame //SR respecto al cual se ejercen
geometry_msgs/Point reference_point //Offset respecto al SR
float64 x
float64 y
float64 z
geometry_msgs/Wrench wrench //Fuerzas y momentos
geometry_msgs/Vector3 force
float64 x
float64 y
float64 z
geometry_msgs/Vector3 torque
float64 x
float64 y
float64 z
time start_time //Instante de aplicación duration
duration //Duración de aplicación
---
bool success //Devuelve true si no hubo error
string status_message
2. /gazebo/spawn_urdf_model : Permite cargar modelos de robots u

objetos en tiempo de ejecución, en nuestro caso, desde un programa en
C++. El tipo de mensaje asociado a este servicio es SpawnModel.srv :
string model_name //Nombre modelo en Gazebo

model_xml //Toda la descripción URDF como string
robot_namespace //Espacio de nombre si se desea
geometry_msgs/Pose initial_pose //Localización inicial
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
string reference_frame //SR para la localización inicial
---
3. gazebo/delete_model : Permite eliminar modelos de robots u objetos

en tiempo de ejecución y su tipo de mensaje asociado es DeleteMol-
del.srv :
string model_name //Nombre del modelo en Gazebo

---
4. /gazebo/clear_body_wrenches: Permite eliminar todas las fuer-

zas aplicadas sobre un cuerpo (link ) y su tipo de mensaje es BodyRe-
quest.srv :
GAZEBO 115
string model_name //Nombre del modelo en Gazebo

---
3.2.4 Plugins para Gazebo

Los plugins de Gazebo son por definición complementos que se añaden
al simulador, o más bien a los modelos, proporcionándole funciones y/o sen-
sores, lo que en ROS equivalen a topics con información relevante sobre la
simulación del robot. En general, salvo los plugins creados para unos paque-
tes específicos, todos se encuentran en el paquete gazebo_plugins en forma
de librería (.so).
Estos plugins se añaden a la descripción URDF del robot entre dos

etiquetas que identifiquen que se refieren al simulador: <gazebo>. Luego
mediante la etiqueta <controller:tipo_plugin> se le asigna un nombre, se
le especifica la librería a usar y se seleccionan los valores para los posibles
parámetros del plugin:
<gazebo>
<cotroller:tipo_plugin name = "..." plugin = "...">
<opción_1>...</opción_1>
(...)
<opción_n>...</opción_n>
</controller:tipo_plugin>
</gazebo>
Existen muchos plugins en el paquete gazebo_plugins, sin embargo aquí
vamos a centrarnos en los cuatro que se han utilizado para nuestras simu-
laciones:
1. GazeboRosTime: Plugin para publicar el tiempo de simulación vía
topic /clock. Su descripción XML es la siguiente:
<controller:gazebo_ros_time name="sim_time"
plugin="libgazebo_ros_time.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
</controller:gazebo_ros_time>
Este plugin al lanzar Gazebo no haría falta, ya que éste también hace
que se publique /clock. Sin embargo, se aconseja añadirlo para usar el
paquete pr2_controller_manager debido a que el plugin inicia un nodo
que controle el tiempo en caso que no exista ninguno como Gazebo.
Sus opciones son, en orden de aparición, la activación o desactivación
y la frecuencia (en Hz ) del topic /clock.
2. GazeboRosP3D: Plugin para la simulación de un sensor de posición
(GPS por ejemplo). Publica la localización de un cuerpo (link ) durante
la simulación vía topic de tipo de mensaje Odometry. Su descripción

es de la forma:
<controller:gazebo_ros_p3d name="sensor_pos"
plugin="libgazebo_ros_p3d.so">
<updateRate>100</updateRate>
<bodyName>link_final</bodyName>
<frameName>link_base</frameName>
<topicName>sensor_odometria</topicName>
<gaussianNoise>0.0</gaussianNoise>
</controller:gazebo_ros_p3d>
Las dos primeras opciones son las mismas que en el plugin anterior.
Las dos siguientes serían el cuerpo (body) del que se lee la posición/o-
rientación y el sistema de referencia (frame) respecto al cual se mide.
Después se elige el nombre del topic de salida, y por último si se desea
añadir ruido a la medida.
El tipo de mensaje Odometry pertenece al paquete nav_msgs y tiene

la siguiente estructura (versión extendida):
Header header
uint32 seq
time stamp
string frame_id
string child_frame_id geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
GAZEBO 117
float64 z
Este tipo de mensaje se compone básicamente de tres partes:
Cabecera (header ): Contiene el instante de simulación y el frame

respecto al cual se midió.
Localización (pose): Posición y orientación del cuerpo respecto al
frame. Este es realmente el que nos interesa.
Velocidad (twist): Velocidad lineal y angular del cuerpo respecto
al frame. (No se usará)
3. GazeboRosF3D: Plugin que publica las fuerzas externas que se estén

aplicando sobre un cuerpo en simulación vía topic de tipo de mensaje
WrenchStamped. Su descripción es:
<controller:gazebo_ros_f3d name="sensor_F"
plugin="libgazebo_ros_f3d.so">
<updateRate>10</updateRate>
<bodyName>link_base</bodyName>
<frameName>link_base</frameName>
<topicName>esfuerzo_base</topicName>
<interface:position name="interfaz_interna" />
</controller:gazebo_ros_f3d>
Las opciones tienen un significado equivalente al plugin anterior. La

única nueva que aparece es <interface>, la cual no tiene mayor im-
portancia para su funcionamiento, pero es necesaria definirla con un
nombre internamente para el plugin.
El tipo de mensaje WrenchStamped pertenece al paquete geometry_msgs,

que ya ha sido explicado en el apartado de aplicaciones y librerías de
cliente de ROS.
4. GazeboRosCamera: Plugin que nos proporciona la simulación de
una cámara instalada en el lugar que queramos del robot o del entorno.
Publicará vía topics las imágenes obtenidas y las especificaciones de
las imágenes de la cámara. La descripción es de la forma:
<sensor:camera name="Camara_sensor">
<imageSize>640 480</imageSize>
<nearClip>0.01</nearClip>
<farClip>10</farClip>
<controller:gazebo_ros_camera name="Camara_controller"
plugin="libgazebo_ros_camera.so">
<updateRate>100.0</updateRate>
<imageTopicName>/ Camara/image_raw</imageTopicName>
<cameraInfoTopicName>/Camara/camera_info</cameraInfoTopicName>
<frameName>Camara</frameName>
</controller:gazebo_ros_camera>
</sensor:camera>
Nótese que el controlador esta vez, además de estar entre las etique-
tas de <gazebo>, está también dentro de las etiquetas de <sensor>,
donde además del controlador se está especificando la resolución, las
distancias más cercana y más lejana a las que los objetos aparecen en
la imagen. Las opciones del controlador son equivalentes a las de los
anteriores plugins salvo que ahora se publican dos topics:
/camera_info: por el que se envían mensajes del tipo CameraInfo

del paquete sensor_msgs con información sobre las especificacio-
nes de las imágenes que se están tomando.
/image_raw : por el que se envía la imagen propiamente dicha con
el tipo de mensaje Image, congruente con el visor de imágenes que
se usará vía ROS: image_view del paquete del mismo nombre.
También puede visualizarse desde rviz pero la carga computacio-
nal conjunta de éste y Gazebo es demasiado alta.

3-Entorno de Simulaciã N

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

3-Entorno de Simulaciã N

Cargado por

Copyright:

Formatos disponibles

Capítulo 3

En este capítulo se hará primero una introducción a ROS, en la que se

Tras esto, nos centraremos en el simulador que se va a utilizar dentro

Todo el contenido de este capitulo es una recopilación de la información

Su filosofía es la de que todos los programas sean de uso libre, reutiliza-

otras plataformas software abiertas ya existentes como OpenCV, Player/S-

ROS se compone de un conjunto de librerías de programación, aplica-

Figura 3.1: Relación ROS y SO

Su estructura es modular, de forma que cada programa o aplicación,

Para comprobar que ROS representa un sistema completo de desarrollo

Figura 3.2: Robot PR2

cumplamos con las interfaces necesarias.

Dado que ROS está continuamente evolucionando, existen varias distri-

ROS Fuerte (Abril, 2012)

ROS Electric (Agosto, 2011)

ROS Diamondback (Marzo, 2011)

ROS C Turtle (Agosto, 2010)

ROS Box Turtle (Marzo, 2010)

En este proyecto se ha utilizado la distribución Electric, dado que era la

Por último, ha de comentarse la existencia de un foro oficial de ROS

3.1.2 Conceptos básicos

1. Paquetes (packages): Son el nivel más bajo de organización del sistema

Figura 3.3: Paquetes

2. Pilas (Stacks): Son agrupaciones de paquetes que forman una librería

Figura 3.4: Pilas

Ambos poseen unos archivos de información especificando lo que contie-

A la hora de ser descargados e instalados, las pilas se agrupan en repo-

Figura 3.5: Repositorios

Por defecto, todo el sistema de ROS se instala en la dirección /opt/ros

Para entender más adelante como se organizan los paquetes de nuestras

rosbuild_add_executable(ejecutable src/programa.cpp): crea en la car-

Para compilar un paquete, basta con hacer en una terminal:

Ésto compilará todo lo especificado en CMakeLists.txt, además de to-

Figura 3.6: Nodos

Los nodos se pueden escribir en C++ o en phyton usando su librería de

roscpp: librería de cliente de C++.

Es muy importante tener en cuenta que la ejecución y comunicación

Los programas de los nodos pertenecientes a este proyecto se escribirán

La programación de todos los nodos en ROS debe incluir las siguientes

int main(int argc, char **argv)

“ros::init(argc, argv, “nombre_nodo”)” inicializa el nodo en el sistema con

“ros::NodeHandle n” crea el principal punto de acceso a las comunicacio-

“ros::ok()” es una función que devuelve un valor falso cuando se dé se

“r.sleep()” es la función que nos asegura que se cumpla el ciclo en 10 Hz

Figura 3.7: Topic[1]

mismo topic ni sabrá si sus mensajes están siendo recibidos. Si se desea

Por un topic se envían objetos de un tipo de mensaje definido en el mis-

ros::Publisher chatter_pub = n.advertise<std_msgs::String

La primera instrucción incluye las definiciones de la clase del tipo de

el topic desde el programa (o nodo cuando esté en ejecución). La terce-

Aunque no se ha dicho, es obvio que el codigo anterior, en general, irá

void chatter_funcion(const std_msgs::String msg)

int main(int argc, char **argv)

La instrucción anterior para la suscripción al topic, no hace por sí sola

pero que no se han usado en este proyecto.

Típicamente, ros::spinOnce()"siempre vá dentro de un bucle while() co-

Figura 3.8: Servicio[1]

Así un nodo puede ofrecer un servicio (server ) y cualquier otro nodo

El servicio ofrecido por un nodo es totalmente independiente de los clien-

bool fun_sum(Matematicas::SumaDosEnteros::Request &req,

ros::ServiceServer service = n.advertiseService("

La primera instrucción incluiría el tipo de servicio que se va a usar,

Equivalentemente para usar un servicio creamos un cliente a éste de la