robotica.unileon.es - User contributions [en]

User:Fernando

2017-05-22T09:48:52Z

Fernando:

* '''Contact:''' fcasag00[ at] estudiantes [dot] unileon [dot] es
* '''Projects:'''
** [[Mobile manipulation]]
** [[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
* [http://fernando.casadogarcia.es Web]
* [http://es.linkedin.com/pub/fernando-casado-garc%C3%ADa/40/bb0/359 LinkedIn]

RoboCup

2016-04-01T08:25:53Z

Fernando:

__TOC__

This page is currently being developed

= Team =

== Robotics Groups ==

[[Image:BotyBot.png|left|300px|thumb|Robotics Group of University of León - [http://robotica.unileon.es/ Team Web Page] [http://unileon.es University Web Page]]]

[[Image:Peloto.png|center|250px|thumb|Robotics Group of Rey Juan Carlos University - [http://www.robotica.gsyc.es/ Team Web Page] [http://www.urjc.es/ University Web Page]]]

== People ==

* Integration (Team Leader): [http://robotica.unileon.es/lera/ Francisco Javier Rodríguez Lera]
* Navigation and Control: [http://gsyc.es/~fmartin/ Francisco Martín Rico]
* Manipulation: [http://www.fernando.casadogarcia.es Fernando Casado García], Alvaro Moreno García,
* Navigation: Jesús Balsa Comerón, Jonathan Gines Claveo, Francisco Javier Gutierrez-Maturana
* Perception: Diego García Ordás, Alvaro Moreno García, Francisco Javier Gutierrez-Maturana
* Team Director: [http://robotica.unileon.es/vmo Vicente Matellán Olivera]

= Robot =

== OrBiOne ==

OrBiOne is the robot that we will be using for 2016 competitons. This mobile manipulation ([http://www.robotnik.eu/robotnik-introduces-rb-one-new-mobile-manipulator/ RB-1]) is developed by [http://www.robotnik.eu/robotnik-launches-the-new-version-of-its-mobile-manipulator-rb-1-and-rb-1-base-mobile-platform/ Robotnik].

[[Image:RB1_robot.jpg|center|border|300px]]

== Hardware Description ==
– Robot name: RB1
#. Base: Robotnik (Dinamixel Motors)
#. Vision Sensors: RGB-D Xtion
#. Range sensors: Frontal laser
#. Dialogue Sensors: Directional microphone Rode Rycote , Loudspeakers
#. Manipulation: 7DOF arm. Dynamixel Pro servos. Workspace: 70 cm
#. Computer Description: Intel Core i7 with 8 GB of RAM and a 200 GB of disk
#. Torso: +40
#. Head: pan-tilt head.

== Software Description ==
– Development framework: ROS

#. Vision: ork, findobject
#. Dialogue: Pocketsphinx
#. BellRecognition: pyaudio
#. Manipulation: MoveIt, self developements.
#. Navigation: Ros stack, self developements.

– Robot Control: Finite State Machine (FSM) using Bica

= Videos =

=== Navigation in indoor environment ===

The video presents a surveillance task in our lab. It shows the navigation approach of OrbiOne robot ([http://www.robotnik.eu/robotnik-introduces-rb-one-new-mobile-manipulator RB1-robotnik robot]) in our apartment.

The robot navigates different apartment rooms in order to check user availability (living room and bedroom), visits (apartment front door after the doorbell rings) or alarm situation (fridge open-door alarm).

<center><videoflash>8icVWz0zSMw</videoflash></center>

=== Quiz Game and Context Awareness===

Trivia game between user and robot. The conversation is interrupted by a ringbell.

<center><videoflash>pU4avjbgFMQ</videoflash></center>



= Research =

== Arhictecture ==

We have a three layer architecture based on ROS and BICA [1]. The lower layer corresponds to ROS, and is in charge of hardware management. The intermediate layer provides the robot skills in order to carry out specific duties (perception, navigation,...).
The top layer presents BICA. It is a component-based for generating behaviors architecture. This node coordinates the various capabilities of the robot depending on the task to be carried out by the robot. Figure 2 shows the overall architecture of the software we have developed for participating in the RoCKIn competition.

[[Image:Rokinarq.jpg|center|border|500px]]

= Github Repository=

* Individuals:
https://github.com/FranLera

https://github.com/fmrico

https://github.com/casadofer

* Group
https://github.com/Robotica-ule/MYRABot

= Papers =

* [http://robotica.unileon.es/mediawiki/documents/TDP_LeonRobot_31032016.pdf Team Description paper ]

* List of group publications can be found in the University of [[Publications |León's robotics group publications page]] and in the URJC's [http://gsyc.es/~fmartin/#publications publications] page.

Mobile manipulation

2016-04-01T08:23:56Z

Fernando:

* '''Project Name:''' Brazo Bioloid controlado por ROS con interfaz Arduino
* '''Author:''' [http://www.fernando.casadogarcia.es Fernando Casado García]
* '''Dates:''' September 2013 -
* '''Degree:''' PhD
* '''Tags:''' MYRAbot, manipulation, arm, Bioloid, webcam, recognition, simulation
* '''Technologies:''' ROS, c++, Dynamixel, Arduino, find_object_2d, gazebo, URDF, moveIt!, actionlib, maxon, epos2, EposManager (wpi-rover)
* '''State:''' Ongoing

----
----
[[File:myrabot_portada.png|100px|thumb|right|MYRABot]]

<center style="font-size: 30px;">MYRABot</center>

{| style="border: solid 0px white; width: 100%"
! Español
! English
|-
| valign="top" width="50%" |Contenido
----

#[[Control brazo MYRAbot (bioloid+arduino)]]
#[[Detección y cálculo de posición de objetos (cámara web)]]
#[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]
#[[Modelo para simulación MYRAbot (urdf+gazebo)]]
#[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]
#[[Órdenes y confirmación mediante voz (sphinx+festival)]]
#[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |Content
----

#[[MYRAbot's arm control (bioloid+arduino)]]
#[[Objects recognition and position calculation (webcam)]]
#[[MYRAbot's arm model for simulation (urdf+gazebo)]]
#[[MYRAbot model for simulation (urdf+gazebo)]]
#[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]
#[[Voice control (sphinx+festival)]]
#[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
|}

MYRABot ROS packages:

[https://github.com/Robotica-ule/MYRABot MYRABot's GitHub]

----
----
[[File:cervantes_portada.png|100px|thumb|right|CeRVaNTeS]]

<center style="font-size: 30px;">CeRVaNTeS</center>

{| style="border: solid 0px white; width: 100%"
! Español
! English
|-
| valign="top" width="50%" |Contenido
----

#[[Control brazo y base CeRVaNTeS (maxon+epos2)]]
#[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]
#[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]
#[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]
#[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |Content
----

#[[CeRVaNTeS' arm and base control (maxon+epos2)]]
#[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]
#[[CeRVaNTeS model for simulation (urdf+gazebo)]]
#[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]
#[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
|}

----
----
[[File:rb1_portada.png|100px|thumb|right|OrBiOne]]

<center style="font-size: 30px;">OrBiOne</center>

Mobile manipulation

2015-11-06T11:31:17Z

Fernando:

* '''Project Name:''' Brazo Bioloid controlado por ROS con interfaz Arduino
* '''Author:''' [http://www.fernando.casadogarcia.es Fernando Casado García]
* '''Dates:''' September 2013 -
* '''Degree:''' PhD
* '''Tags:''' MYRAbot, manipulation, arm, Bioloid, webcam, recognition, simulation
* '''Technologies:''' ROS, c++, Dynamixel, Arduino, find_object_2d, gazebo, URDF, moveIt!, actionlib, maxon, epos2, EposManager (wpi-rover)
* '''State:''' Ongoing

----
----
[[File:myrabot_portada.png|100px|thumb|right|MYRABot]]

<center style="font-size: 30px;">MYRABot</center>

{| style="border: solid 0px white; width: 100%"
! Español
! English
|-
| valign="top" width="50%" |Contenido
----

#[[Control brazo MYRAbot (bioloid+arduino)]]
#[[Detección y cálculo de posición de objetos (cámara web)]]
#[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]
#[[Modelo para simulación MYRAbot (urdf+gazebo)]]
#[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]
#[[Órdenes y confirmación mediante voz (sphinx+festival)]]
#[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |Content
----

#[[MYRAbot's arm control (bioloid+arduino)]]
#[[Objects recognition and position calculation (webcam)]]
#[[MYRAbot's arm model for simulation (urdf+gazebo)]]
#[[MYRAbot model for simulation (urdf+gazebo)]]
#[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]
#[[Voice control (sphinx+festival)]]
#[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
|}

MYRABot ROS packages:

[https://github.com/Robotica-ule/MYRABot MYRABot's GitHub]

----
----
[[File:cervantes_portada.png|100px|thumb|right|CeRVaNTeS]]

<center style="font-size: 30px;">CeRVaNTeS</center>

{| style="border: solid 0px white; width: 100%"
! Español
! English
|-
| valign="top" width="50%" |Contenido
----

#[[Control brazo y base CeRVaNTeS (maxon+epos2)]]
#[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]
#[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]
#[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]
#[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |Content
----

#[[CeRVaNTeS' arm and base control (maxon+epos2)]]
#[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]
#[[CeRVaNTeS model for simulation (urdf+gazebo)]]
#[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]
#[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
|}

----
----
[[File:rb1_portada.png|100px|thumb|right|RB-1]]

<center style="font-size: 30px;">RB-1</center>

File:Rb1 portada.png

2015-11-06T11:27:25Z

Fernando: rb1

rb1

Integración de MYRAbot en moveIt! (gazebo+moveIt!)

2015-10-28T16:01:23Z

Fernando: /* Pasos previos= */

Fernando-TFM-ROS02

2014-11-13T12:22:58Z

Fernando: /* =Calibración de odómetro y giroscopio */

* '''Project Name:''' Programas de control para el robot Turtlebot sobre ROS
* '''Authors:''' [http://www.fernando.casadogarcia.es Fernando Casado García]
* '''Academic Year:''' 2011-2012
* '''Degree:''' Master
* '''Tags:''' Turtlebot
* '''Technologies:''' ROS, c++
* '''Status:''' Finished

----
{| style="border: solid 0px white; width: 100%"
!Otros artículos
----
|-
| valign="top" width="100%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
|}
----

=Instalando ROS=

Lo primero que necesitamos es un computador con el sistema operativo [http://www.ubuntu.com Ubuntu], que es el único soportado por [http://www.ros.org ROS], o instalarlo; otros sistemas operativos para los que está disponible solo son experimentales. Para instalar [http://www.ubuntu.com Ubuntu] en un equipo con sistema operativo windows, podemos optar entre una instalación tradicional desde un CD-ROM o USB, en una partición diferente a la de windows, o la opción más rápida y sencilla , instalarlo desde windows como si se tratara de una aplicación o programa mas.

Provistos ya de un equipo con [http://www.ubuntu.com Ubuntu], para descargar e instalar el sistema [http://www.ros.org ROS] simplemente deberemos seguir las indicaciones de [http://www.ros.org/wiki/electric/Installation/Ubuntu instalación]. No esta de más el tomarse un tiempo realizando los [http://www.ros.org/wiki/ROS/Tutorials tutoriales] que proponen, son de gran ayuda para conocer la estructura de ficheros y el funcionamiento de ''Nodes'' y ''Topics''.

==Instalando ''Turtlebot-desktop''==

Con el sistema [http://www.ros.org ROS] instalado, necesitamos instalar las herramientas para desarrollo del robot [http://www.turtlebot.com Turtlebot] en el equipo de trabajo. Ejecutando el siguiente comando en un terminal comenzará la descarga e instalación:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-desktop</syntaxhighlight>

Es importante que el reloj del PC estación de trabajo se encuentre sincronizado con el reloj del PC robot, para prevenir las posibles perdidas de red inalámbrica. Para instalar el cliente [http://es.wikipedia.org/wiki/Network_Time_Protocol NTP] ''Chrony'' se ejecutará el siguiente comando en un terminal:

<syntaxhighlight>sudo apt-get install chrony</syntaxhighlight>

==Instalando ''Turtlebot-robot''==

Puede que se utilice otro equipo diferente al que suministran con el [http://www.turtlebot.com Turtlebot] o que se actualice [http://www.ubuntu.com Ubuntu] a una nueva versión, por lo que será necesario instalar la versión para el robor de [http://www.ros.org ROS]. Ejecutando el siguiente comando en un terminal comenzará la descarga e instalación:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-robot</syntaxhighlight>

Se necesita crear un archivo llamado “52-turtlebot.rules” en el directorio '''/etc/udev/rules.d/'''. Con el siguiente contenido:

<syntaxhighlight>ATTRS{idProduct}=="XXXX",ATTRS{idVendor}=="XXXX",MODE="666",GROUP="turtlebot"</syntaxhighlight>

Donde las “XXXX” corresponden a la identificación del producto y del fabricante, respectivamente, del adaptador USB de conexión con el iRobot Create. Esta información la obtiene ejecutando el siguiente comando en un terminal, con el adaptador conectado:

<syntaxhighlight>lsusb</syntaxhighlight>

Que mostrará una información similar a esta:
<syntaxhighlight>
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 002 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 003 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 004 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 005 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 002 Device 002: ID 0a5c:2101 Broadcom Corp. Bluetooth Controller
Bus 003 Device 002: ID 04fc:0801 Sunplus Technology Co., Ltd
Bus 004 Device 002: ID 08ff:2580 AuthenTec, Inc. AES2501 Fingerprint Sensor
Bus 004 Device 003: ID 0403:6001 Future Technology Devices International, Ltd FT232 USB-Serial (UART) IC
</syntaxhighlight>

Donde “6001” corresponde a la identificación del producto y “0403” a la identificación del fabricante.
También se debe añadir el usuario, con el que hemos instalado ROS, al grupo “dialout”, para que tenga permisos sobre el puerto que se emplea para la comunicación con el iRobot Create, ttyUSB0.

=Conociendo el Turtlebot=

[[File:Turtlebot.png|thumb|130px|Fotografía del [http://www.turtlebot.com Turtlebot].]]

==Hardware==

* [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create]
* Laptop ASUS 1215N
* Microsoft Kinect
* Placa de gestión de alimentación y giroscopio
* Cables USB de comunicaciones y alimentación
* Estructura de soporte de elementos

==Software==

* Sistema operativo [http://www.ubuntu.com Ubuntu]
* [http://www.ros.org ROS] con ''Turtlebot-robot''

=Conexión estación de trabajo-robot=

==Configuración de la red==

Lo habitual es conectar ambos equipos a una red [http://es.wikipedia.org/wiki/Wi-Fi Wi-Fi], pero puede darse que el PC estación de trabajo no disponga de un interfaz de red inalámbrica. En este caso es necesario poder conectarse a un puerto del [http://es.wikipedia.org/wiki/Router router] que provee la red [http://es.wikipedia.org/wiki/Wi-Fi Wi-Fi], para que exista una ruta de acceso entre el PC estación de trabajo y el PC robot.

En ambos equipos, PC estación de trabajo y PC robot, se añadirá el ROS_MASTER_URI en el archivo de configuración del terminal (~/.bashrc), que apunta a la dirección IP del PC robot, que es el que ejecuta el servicio maestro. Simplemente se debe escribir el siguiente comando en un terminal, sustituyendo "IP_OF_TURTLEBOT", por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>echo export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311 >> ~/.bashrc</syntaxhighlight>

También se debe asignar un nombre de equipo, tanto al PC estación de trabajo como al PC robot. Este nombre será la dirección IP de cada equipo, que se almacenará en el archivo de configuración del terminal (~/.bashrc). En un terminal del PC robot se ejecutará el siguiente comando, sustituyendo "IP_OF_TURTLEBOT", por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>echo export ROS_HOSTNAME=IP_OF_TURTLEBOT >> ~/.bashrc</syntaxhighlight>

En un terminal del PC estación de trabajo se ejecutará el siguiente comando, sustituyendo "IP_OF_WORKSTATION", por la dirección IP asignada en la red al PC estación de trabajo:

<syntaxhighlight>echo export ROS_HOSTNAME=IP_OF_WORKSTATION >> ~/.bashrc</syntaxhighlight>

* '''Nota''': Para conocer la dirección IP, asignada en la red a un equipo, simplemente se debe ejecutar en un terminal:

<syntaxhighlight>ifconfig</syntaxhighlight>

que mostrará una información similar a esta:

<syntaxhighlight>
eth0 Link encap:Ethernet direcciónHW 00:16:d4:55:85:b4
ACTIVO DIFUSIÓN MULTICAST MTU:1500 Métrica:1
Paquetes RX:0 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:0 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:1000
Bytes RX:0 (0.0 B) TX bytes:0 (0.0 B)
Interrupción:44

lo Link encap:Bucle local
Direc. inet:127.0.0.1 Másc:255.0.0.0
Dirección inet6: ::1/128 Alcance:Anfitrión
ACTIVO BUCLE FUNCIONANDO MTU:16436 Métrica:1
Paquetes RX:64 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:64 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:0
Bytes RX:5184 (5.1 KB) TX bytes:5184 (5.1 KB)

wlan0 Link encap:Ethernet direcciónHW 00:18:de:21:4b:ad
Direc. inet:192.168.1.10 Difus.:192.168.1.255 Másc:255.255.255.0
Dirección inet6: fe80::218:deff:fe21:4bad/64 Alcance:Enlace
ACTIVO DIFUSIÓN FUNCIONANDO MULTICAST MTU:1500 Métrica:1
Paquetes RX:130972 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:87840 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:1000
Bytes RX:169415208 (169.4 MB) TX bytes:9379691 (9.3 MB)
</syntaxhighlight>

==Puesta en marcha==

Con la red configurada, ya se puede proceder a la puesta en marcha del [http://www.turtlebot.com Turtlebot]. Se conectarán los cables USB de comunicación y alimentación del robot al PC robot, se encenderá el [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create] y se arrancará el PC robot.

Las operaciones sobre el PC robot se pueden hacer directamente sobre él, pero no es la opción recomendable por comodidad de trabajo. Así que se comenzará conectando por [http://es.wikipedia.org/wiki/Secure_Shell ssh], desde el PC estación de trabajo con el PC robot. Se ejecutará el siguiente comando en un terminal en la estación de trabajo, sustituyendo "IP_OF_TURTLEBOT" por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>ssh turtlebot@IP_OF_TURTLEBOT</syntaxhighlight>

Pedirá la contraseña asignada al PC robot. Cuando se conecte el [http://es.wikipedia.org/wiki/Prompt prompt] del terminal cambiará por el del PC robot. Se puede comprobar el estado del servicio del robot, tecleando el siguiente comando en el terminal:

<syntaxhighlight>sudo service turtlebot status</syntaxhighlight>

Devolverá un mensaje diciendo que está iniciado. El servicio se inicia siempre que encendemos el PC robot, pero se puede detener y poner en marcha desde el terminal con los siguientes comandos:

<syntaxhighlight>sudo service turtlebot stop
sudo service turtlebot start</syntaxhighlight>

En un terminal nuevo, de forma local sin conectar con el PC robot, se ejecutará la aplicación del tablero de instrumentos (dashboard) del robot. Simplemente se debe introducir este comando en el terminal:

<syntaxhighlight>rosrun turtlebot_dashboard turtlebot_dashboard&</syntaxhighlight>

Aparecerá una pequeña ventana similar a esta:

[[file:turtlebot_dashboard_0.png|center]]

La ventana del tablero de instrumentos consta de tres zonas bien diferenciadas:
* Diagnóstico del sistema, mensajes del sistema y modo de operación.
* Interruptores de los circuitos de las tres salidas digitales del [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create].
* Niveles de batería de el [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create] y el PC robot.
Si todo va bien, como en la imagen, estarán en verde los iconos de diagnostico del sistema (llave inglesa) y mensajes del sistema (nube de llamada). El modo de operación se debe poner poner en ''Full Mode'', para poder operar el robot. En los otros modos no está permitida la operación del robot, son el modo de carga (Passive Mode) y de espera (Safety Mode).

[[file:turtlebot_dashboard.png|center]]

También se debe activar al menos el interruptor "1", que es el que controla el circuito de alimentación de la cámara Kinect. Ahora ya esta todo listo para comenzar a operar con el robot. Para verlo en funcionamiento es recomendable probar alguna de las [http://www.ros.org/wiki/turtlebot_apps aplicaciones] de demostración.

=Primer programa (Vuelta al mundo)=

==Creando un espacio de trabajo==

Lo más cómodo para trabajar es crear un directorio propio para ''stacks'' y ''packages'' en el PC estación de trabajo, ya que [http://www.ros.org ROS] tiene por defecto como directorio para ''stacks'' y ''packages'' '''/opt/ros/electric/stacks'''. Se pueden crear los nuevos ''stacks'' o ''packages'' en este directorio por defecto, pero queda más organizado y accesible de esta forma.

El directorio se puede situar donde y con el nombre que se quiera. Se comenzará creando un directorio, por ejemplo en la carpeta personal:

<syntaxhighlight>mkdir ~/ros_workspace</syntaxhighlight>

Para que el sistema también busque ''stacks'' y ''packages'' en nuestra carpeta, se debe crear en la carpeta personal un archivo llamado "setup.sh", con el siguiente contenido:
<syntaxhighlight>
#!/bin/sh
source /opt/ros/electric/setup.bash
export ROS_ROOT=/opt/ros/electric/ros
export PATH=$ROS_ROOT/bin:$PATH
export PYTHONPATH=$ROS_ROOT/core/roslib/src:$PYTHONPATH
export ROS_PACKAGE_PATH=~/ros_workspace:/opt/ros/electric/stacks:$ROS_PACKAGE_PATH
</syntaxhighlight>

Se debe añadir este nuevo archivo creado al archivo de configuración del terminal (~/.bashrc), para hacer efectiva esta modificación. Para ello se debe ejecutar el siguiente comando en un terminal:

<syntaxhighlight>echo source ~/setup.sh >> ~/.bashrc</syntaxhighlight>

==Creando un ''package''==

En el directorio de trabajo creado se creará un ''package'', el cual va a depender de otros ''packages'' del sistema. Las dependencias van en función del tipo de mensajes que queremos enviar o recibir, y el lenguaje empleado en los programas. En este primer programa no se necesitan todas las dependencias, pero se usaran en los siguientes programas, por lo que las añadiremos al crear el ''package'' que va a contener los programas. Para esto se introducirá la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>cd ~/ros_workspace
roscreate-pkg turtlebot_fer std_msgs turtlebot_node geometry_msgs nav_msgs sensor_msgs image_transport roscpp</syntaxhighlight>

También se pueden añadir dependencias a un ''package'' ya creado, simplemente se debe editar el archivo "manifest.xml" que se encuentra dentro de la carpeta del ''package'' creado. El contenido del archivo "manifest.xml" es similar al siguiente, las dependencias se encuentran al final:

<syntaxhighlight lang="xml" enclose="div">
<package>
<description brief="turtlebot_fer">

turtlebot_fer

</description>
<author>FeR</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/turtlebot_fer</url>
<depend package="turtlebot_node"/>
<depend package="geometry_msgs"/>
<depend package="nav_msgs"/>
<depend package="std_msgs"/>
<depend package="sensor_msgs"/>
<depend package="image_transport"/>
<depend package="roscpp"/>

</package>

</syntaxhighlight>

Una vez creado el ''package'', o modificadas las dependencias de un ''package'' existente, se debe compilar para que se hagan efectivas las dependencias. Simplemente se debe ejecutar el siguiente comando en el terminal, indicando el nombre del ''package'':

<syntaxhighlight>rosmake turtlebot_fer</syntaxhighlight>

==Escribiendo y compilando el programa==

Los programas se pueden escribir en c++ o python, el escogido ha sido c++. Para la redacción del programa se puede emplear algún editor de código, como [http://www.geany.org/ Geany]. Los archivos de los programas se guardarán en la carpeta "src" del ''package'' creado, turtlebot_fer. Este primer programa, simplemente crea el ''nodo'' "gira" y publica el ''topic'' "cmd_vel", que hace mover el robot con una velocidad de avance y de giro, describiendo un círculo, hasta que se reciba la señal de parada "Ctrl+C". El programa es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"

int main(int argc, char **argv)
{

ros::init(argc, argv, "gira"); //Creación del nodo "gira"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

geometry_msgs::Twist vel;

ros::Rate loop_rate(10); //Frecuencia de realización del bucle (10 Hz)

while (ros::ok()) //Bucle mientras no se reciba "Ctrl+C"
{
vel.linear.x = 0.2; //velocidad de avance
vel.angular.z = 0.4; //velocidad de giro

vel_pub_.publish(vel);

loop_rate.sleep();
}

return 0;
}

</syntaxhighlight>

Se guardará con el nombre "gira.cpp". Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight>rosbuild_add_executable(gira src/gira.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd turtlebot_fer
make</syntaxhighlight>

==Ejecutando el programa==

===Simulador===

Lo primero que hay que hacer es descargar el simulador del [http://www.turtlebot.com Turtlebot], ejecutando el siguiente comando en unterminal:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-simulator</syntaxhighlight>

Se puede ejecutar primero en el simulador para salvaguardar la integridad del robot. Lo primero que se debe hacer para poder arrancar el simulador es comentar las líneas que se han añadido para la comunicación con el PC robot en el archivo "~/.bashrc", del PC estación de trabajo, tal y como se muestra, añadiendo el símbolo "#" al principio de la línea:

<syntaxhighlight># export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311
# export ROS_HOSTNAME=IP_OF_WORKSTATION</syntaxhighlight>

Se debe reiniciar la sesión de usuario para que surtan efecto los cambios. Para ejecutar el simulador del [http://www.turtlebot.com Turtlebot] simplemente se bebe ejecutar el siguiente comando en un terminal:

<syntaxhighlight>roslaunch turtlebot_gazebo turtlebot_empty_world.launch</syntaxhighlight>

Terminada la carga aparecerá una ventana gráfica del simulador [http://gazebosim.org/ Gazebo] con un escenario vacío y el modelo 3D del robot, como se muestra en la imagen:

[[file:turtlebot_sim.png|center]]

Se puede cambiar el punto de vista jugando con la rueda del ratón y el botón derecho. Además, se pueden añadir objetos 3D, como esferas, prismas y cilindros. Para ejecutar el programa creado ejecutamos el siguiente comando en un terminal nuevo, indicando el ''package'' donde se encuentra y el nombre del ejecutable:

<syntaxhighlight>rosrun turtlebot_fer gira</syntaxhighlight>

Si todo ha ido bien, el modelo 3D del robot comenzara a desplazarse formando un circulo, y no se detendrá hasta que reciba la señal de parada el terminal desde el que se ejecuta el programa, "Ctrl+C".

===Robot===

Si se han modificado las lineas para comunicación del archivo "~/.bashrc", para ejecutar el simulador, se deben restaurar y reiniciar la sesión de usuario para que surtan efecto los cambios. Ahora se procederá a la [[#Puesta en marcha|puesta en marcha]] del robot.

Conectado el PC estación de trabajo al PC robot, se debe [[#Creando un espacio de trabajo|crear un espacio de trabajo]] y [[#Creando un package|crear un ''package'']], al igual que en el PC estación de trabajo. Creado el espacio de trabajo y el ''package'', se copiará el programa situado en el PC estación de trabajo al PC robot, escribiendo el siguiente comando en un terminal del PC estación de trabajo:

<syntaxhighlight>roscd turtlebot_fer
cd src
scp gira.cpp turtlebot@IP_OF_TURTLEBOT:/home/turtlebot/ros_workspace/turtlebot_fer/src/gira.cpp</syntaxhighlight>

Una vez copiado el archivo del programa al PC robot, para poder ser ejecutado hay que [[#Escribiendo y compilando el programa|compilarlo]] desde un terminal del PC robot. Para ejecutar el programa simplemente se debe introducir el siguiente comando en un terminal del PC robot, tomando la precaución que el robot se encuentre en un espacio lo bastante diáfano para evitar colisiones:

<syntaxhighlight>rosrun turtlebot_fer gira</syntaxhighlight>

En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>ZY1oZWB7Vn4</videoflash></center>

<center>[http://www.youtube.com/watch?v=ZY1oZWB7Vn4 Ver vídeo en YouTube]</center>

* '''Nota''': si se encuentra conectada al robot alguna de las fuentes de alimentación, PC robot o [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create], este no se moverá.

=Segundo programa (Golpea y escapa)=

Este segundo programa añade la suscripción a un ''topic'', además de la publicación. El funcionamiento es bastante simple, comenzará a avanzar en línea recta hasta que se detecte el accionamiento del parachoques del [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create], provocado por el choque con un objeto; en este momento realizará un pequeño retroceso y girará un ángulo, de sentido y magnitud aleatorios, para intentar evitar el obstáculo. No se detendrá hasta recibir la señal de parada "Ctrl+C". El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"
#include "turtlebot_node/TurtlebotSensorState.h" //Librería "TurtlebotSensorState" del package "turtlebot_node"

geometry_msgs::Twist vel; //Declaración de la variable global vel
double alea; //Declaración de la variable global alea

void callback(const turtlebot_node::TurtlebotSensorState& sen) //Función de llamda con cada dato recibido
{
ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Rate loop_rate(10); //Frecuencia de realización del bucle (10 Hz)

ros::Time ahora; //Declaración de variable tipo tiempo, variable ROS

if (sen.bumps_wheeldrops == 0) //Comprobación si ha habido señal del parachoques
{
::vel.linear.x = 0.2;
::vel.angular.z = 0.0;

vel_pub_.publish(::vel);
}
else
{
ahora = ros::Time::now(); //Asignación del tiempo actual a la variable ahora

while (ros::Time::now() < (ahora + ros::Duration(0.5))) //Bucle durante 0,5 segundos, retroceso después de choque
{
::vel.linear.x = -0.2;
::vel.angular.z = 0.0;

vel_pub_.publish(::vel);

loop_rate.sleep();
}

::alea = ((rand() %40) -20)/10; //Asignación de valor aleatorio

while (::alea == 0) //Bucle mientras la variable aleatoria sea cero
{
::alea = ((rand() %40) -20)/10;
}

ahora = ros::Time::now(); //Asignación del tiempo actual a la variable ahora

while (ros::Time::now() < (ahora + ros::Duration(1.5))) //Bucle durante 1,5 segundos, giro de ángulo aleatorio
{
::vel.linear.x = 0.0;
::vel.angular.z = ::alea;

vel_pub_.publish(::vel);

loop_rate.sleep();
}
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "juguete_golpe"); //Creación del nodo "juguete_golpe"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Subscriber odom_sub_= n.subscribe("turtlebot_node/sensor_state", 1, callback); //Suscripción del topic "turtlebot_node/sensor_state"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "juguete_golpe.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]].
En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>w7k0381Nfb8</videoflash></center>

<center>[http://www.youtube.com/watch?v=w7k0381Nfb8 Ver vídeo en YouTube]</center>

* '''Nota''': El modelo 3D del [http://www.turtlebot.com Turtlebot] empleado en el simulador no implementa este sensor, por lo que no será posible probar el programa en el [[#Simulador|simulador]].

=Tercer programa (Toro loco)=

La peculiaridad de este programa es que el ''topic'' de suscripción es la imagen obtenida por la cámara Kinect. La imagen se recibe empaquetada en un array de "píxeles ancho x píxeles alto x 3 (componentes color rgb)" elementos, por defecto (640 x 480 x 3) = 921600 elementos. Los píxeles se encuentran representados en sus componentes rgb, con ese orden, comenzando en el píxel de la esquina superior izquierda, continuando por filas hasta llegar al último píxel, esquina inferior derecha de la imagen.

{| class="wikitable" align="center"
! align = "center" colspan = "3" |Píxel 1 !! align = "center" colspan = "3" |Píxel 2 !! align = "center" colspan = "3" |Píxel 3 !! align = "center" | !! align = "center" colspan = "3" |Píxel 307200
|-
|bgcolor = "red" align = "center" width="40px"| 100 ||bgcolor = "green" align = "center" width="40px" | 65 ||bgcolor = "blue" align = "center" width="40px" | 50 ||bgcolor = "red" align = "center" width="40px" | 110 ||bgcolor = "green" align = "center" width="40px"| 63 ||bgcolor = "blue" align = "center" width="40px"| 49 ||bgcolor = "red" align = "center" width="40px"| 109 ||bgcolor = "green" align = "center" width="40px"| 65 ||bgcolor = "blue" align = "center" width="40px"| 48 ||align = "center" width="40px"| ... ||bgcolor = "red" align = "center" width="40px"| 75 ||bgcolor = "green" align = "center" width="40px"| 98 ||bgcolor = "blue" align = "center" width="40px"| 244
|-
|align = "center"|[0] ||align = "center"|[1] ||align = "center"|[2] ||align = "center"|[3] ||align = "center"|[4] ||align = "center"|[5] ||align = "center"|[6] ||align = "center"|[7] ||align = "center"|[8] ||align = "center"| ||align = "center"|[921597] ||align = "center"|[921598] ||align = "center"|[921599]
|}

Este tercer programa realiza el seguimiento de objetos de color rojo. Para ello analiza cada imagen píxel a píxel, contando los píxeles del lado izquierdo y del lado derecho. Cuando el número total de píxeles de tonalidades rojas es superior al 0,1 %, de los píxeles totales de la imagen, e inferior al 5 %, comienza el seguimiento hasta hacer el numero de píxeles totales cercano al 2,5 % e igual a cada lado (estos valores dependerán del tamaño del objeto). Cuando el objeto se encuentre demasiado lejos o demasiado cerca se detendrá el seguimiento. Para facilitar la calibración cada vez que se quiera modificar alguno de los parámetros del programa se creará un archivo .h, en el que estarán definidas las constantes para cada parámetro, se guardará con el nombre "param_sigue_rojo.h" y con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#ifndef __PARAM_SIGUE_ROJO_H
#define __PARAM_SIGUE_ROJO_H

#define li_red 100 //Límite inferior para canal rojo
#define ls_red 255 //Límite superior para canal rojo
#define li_green 0 //Límite inferior para canal verde
#define ls_green 30 //Límite superior para canal verde
#define li_blue 0 //Límite inferior para canal azul
#define ls_blue 50 //Límite superior para canal azul

#define tam_min 0.001 //Porcentaje de píxeles de tonalidades rojas mínimo
#define tam_max 0.05 //Porcentaje de píxeles de tonalidades rojas máximo
#define tam_med 0.025 //Porcentaje de píxeles de tonalidades rojas medio

#define Px 0.08 //Ganancia velocidad de avance-retroceso
#define Pz 1.2 //Ganancia velocidad de giro

#endif /*__PARAM_SIGUE_ROJO_H*/

</syntaxhighlight>

El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "sensor_msgs/Image.h" //Librería "Image" del package "sensor_msgs"
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"
#include "param_sigue_rojo.h" //Parámetros de ajuste

geometry_msgs::Twist vel; //Declaración de la variable global vel
sensor_msgs::Image region; //Declaración de la variable global vel

void callback(const sensor_msgs::Image& ima)
{
ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

uint pix_r; //Declaración de variables de almacenaje valores rgb de cada píxel
uint pix_g;
uint pix_b;
uint ancho = ima.step; //Número de elementos de cada fila de la imagen
uint tam = ima.data.size(); //Número de elementos del array

double cont_izq = 0; //Declaración e inicialización de contadores de píxeles
double cont_der = 0;

::region = ima;
uint up;
uint right;
uint down;
uint left;

for (uint i = 0; i < tam; i = i + ancho) //Filtro de regiones pequeñas o píxeles aislados
{
for (uint j = 0; j < ancho; j = j + 3)
{
pix_r = ima.data[i + j];
pix_g = ima.data[i + j + 1];
pix_b = ima.data[i + j + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
up = 12;
if ((i/ancho) < 12)
{
up = (i/ancho);
}
right = 12;
if (((ancho - j)/3) < 12)
{
right = ((ancho - j)/3);
}
down = 12;
if (((tam/ancho) - (i/ancho)) < 12)
{
down = ((tam/ancho) - (i/ancho));
}
left = 12;
if ((j/3) < 12)
{
left = (j/3);
}

uint contr = 0;

for (uint k = i + j - (ancho*up) - (3*left); k <= (i + j + (ancho*down) - (3*left)); k = k + ancho)
{
for(uint l = 0; l < ((3*left) + (3*right) +3); l = l + 3)
{
pix_r = ima.data[k + l];
pix_g = ima.data[k + l + 1];
pix_b = ima.data[k + l + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
contr++;
}
}
}
if (contr > (625*0.5))
{
::region.data[i + j] = 1;
::region.data[i + j + 1] = 1;
::region.data[i + j + 2] = 1;
}
}
}
}

for (uint i = 0; i < (tam - (ancho/2)); i = i + ancho) //Contar píxeles tonos rojos lado izquierdo imagen
{
for (uint j = 0; j < (ancho/2); j = j + 3)
{
pix_r = ::region.data[i + j];
pix_g = ::region.data[i + j + 1];
pix_b = ::region.data[i + j + 2];

if (pix_r == 1 && pix_g == 1 && pix_b == 1)
{
cont_izq++;
}
}
}

for (uint i = (ancho/2); i < tam; i = i + ancho) //Contar píxeles tonos rojos lado derecho imagen
{
for (uint j = 0; j < (ancho/2); j = j + 3)
{
pix_r = ::region.data[i + j];
pix_g = ::region.data[i + j + 1];
pix_b = ::region.data[i + j + 2];

if (pix_r == 1 && pix_g == 1 && pix_b == 1)
{
cont_der++;
}
}
}

double cont = cont_izq + cont_der;

if (cont > ((tam/3) * tam_min) && cont < ((tam/3) * tam_max)) //Comprobación si se está dentro del limite inferior y superior de píxeles
{
::vel.angular.z = (cont_izq - cont_der) / (::abs(cont_izq - cont_der) * Pz); //Velocidad de giro
::vel.linear.x = (((tam/3) * tam_med) - cont) / ((tam/3) * Px); //Velocidad de avance-retroceso
vel_pub_.publish(vel);
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "sigue_rojo"); //Creación del nodo "sigue_rojo"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Subscriber image_sub_= n.subscribe("camera/rgb/image_color", 1, callback); //Suscripción del topic "camera/rgb/image_color"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

* '''Nota''': Ya que la tonalidad percibida por la cámara depende de la iluminación del recinto, es necesaria una calibración de los límites de los canales RGB, para un correcto funcionamiento.

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "sigue_rojo.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]]. Para poner en marcha la cámara Kinect se debe lanzar en el PC robot el "kinect.launch", ejecutando el siguiente comando en un terminal:

<syntaxhighlight>roslaunch turtlebot_bringup kinect.launch</syntaxhighlight>

En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>LSXP9Xgl1MI</videoflash></center>

<center>[http://www.youtube.com/watch?v=LSXP9Xgl1MI Ver vídeo en YouTube]</center>

Además para la supervisión de las imagenes procesadas por el programa se ha realizado otro programa que publica el ''topic'' "/camara/rojo" con la imagen modificada, en la que los píxeles de tonalidad roja se colorean en rojo y el resto se muestra en escala de grises, con una linea divisoria de color negro en el centro. El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "sensor_msgs/Image.h" //Librería "Image" del package "sensor_msgs"
#include "param_sigue_rojo.h" //Parámetros de ajuste

sensor_msgs::Image region; //Declaración de la variable global region

void callback(const sensor_msgs::Image& ima)
{
ros::NodeHandle n;

ros::Publisher rojo_pub_=n.advertise<sensor_msgs::Image>("camara/rojo", 1); //Publicación del topic "camara/rojo"

uint pix_r; //Declaración de variables de almacenaje valores rgb de cada píxel
uint pix_g;
uint pix_b;
uint ancho = ima.step; //Número de elementos de cada fila de la imagen
uint tam = ima.data.size(); //Número de elementos del array

::region = ima;
uint up;
uint right;
uint down;
uint left;

for (uint i = 0; i < tam; i = i + ancho) //Filtro de regiones pequeñas o píxeles aislados
{
for (uint j = 0; j < ancho; j = j + 3)
{
pix_r = ima.data[i + j];
pix_g = ima.data[i + j + 1];
pix_b = ima.data[i + j + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
up = 12;
if ((i/ancho) < 12)
{
up = (i/ancho);
}
right = 12;
if (((ancho - j)/3) < 12)
{
right = ((ancho - j)/3);
}
down = 12;
if (((tam/ancho) - (i/ancho)) < 12)
{
down = ((tam/ancho) - (i/ancho));
}
left = 12;
if ((j/3) < 12)
{
left = (j/3);
}

uint contr = 0;

for (uint k = i + j - (ancho*up) - (3*left); k <= (i + j + (ancho*down) - (3*left)); k = k + ancho)
{
for(uint l = 0; l < ((3*left) + (3*right) +3); l = l + 3)
{
pix_r = ima.data[k + l];
pix_g = ima.data[k + l + 1];
pix_b = ima.data[k + l + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
contr++;
}
}
}
if (contr > (625*0.5))
{
::region.data[i + j] = 255;
::region.data[i + j + 1] = 0;
::region.data[i + j + 2] = 0;

}
else //Conversión a escala de grises regiones pequeñas o píxeles aislados
{
if (pix_r > pix_g)
{
::region.data[i + j] = pix_r;
::region.data[i + j + 1] = pix_r;
::region.data[i + j + 2] = pix_r;
}
else if (pix_g > pix_b)
{
::region.data[i + j] = pix_g;
::region.data[i + j + 1] = pix_g;
::region.data[i + j + 2] = pix_g;
}
else
{
::region.data[i + j] = pix_b;
::region.data[i + j + 1] = pix_b;
::region.data[i + j + 2] = pix_b;
}
}
}
else //Conversión a escala de grises resto píxeles
{
if (pix_r > pix_g)
{
::region.data[i + j] = pix_r;
::region.data[i + j + 1] = pix_r;
::region.data[i + j + 2] = pix_r;
}
else if (pix_g > pix_b)
{
::region.data[i + j] = pix_g;
::region.data[i + j + 1] = pix_g;
::region.data[i + j + 2] = pix_g;
}
else
{
::region.data[i + j] = pix_b;
::region.data[i + j + 1] = pix_b;
::region.data[i + j + 2] = pix_b;
}
}
}
}

for (uint i = (ancho/2)-3; i < tam; i = i + ancho) //Línea negra divisoria central
{
for (uint j = 0; j < 6; j = j + 3)
{
::region.data[i + j] = 0;
::region.data[i + j + 1] = 0;
::region.data[i + j + 2] = 0;
}
}

rojo_pub_.publish(::region);
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "visualizacion"); //Creación del nodo "visualizacion"

ros::NodeHandle n;

ros::Publisher rojo_pub_=n.advertise<sensor_msgs::Image>("camara/rojo", 1); //Publicación del topic "camara/rojo"

ros::Subscriber image_sub_= n.subscribe("camera/rgb/image_color", 1, callback); //Suscripción del topic "camera/rgb/image_color"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "visualizacion.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]].

Para poder ejecutar "kinect.launch", "sigue_rojo" y "visualización"de forma simultanea se creará un ''launcher''. Para ello crearemos dentro de la carpeta de nuestro ''package'' una carpeta llamada "launch", dentro de la cual crearemos un archivo llamado "rojo.launch" con el siguiente contenido:
<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />
<node name="sigue_rojo" pkg="turtlebot_fer" type="sigue_rojo" />
<node name="visualizacion" pkg="turtlebot_fer" type="visualizacion" />
</launch>
</syntaxhighlight>

Para ejecutar el ''launcher'' que lanza los dos nodos, "sigue_rojo" y "visualización", simplemente habrá que ejecutar el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_fer rojo.launch</syntaxhighlight>

Para poder visualizar la imagen modificada de la cámara Kinect en el PC estación de trabajo, para la supervisión. Simplemente hay que ejecutar el siguiente comando en un terminal:

<syntaxhighlight>rosrun image_view image_view image:=/camara/rojo</syntaxhighlight>

Las imágenes tendrán una tasa de frames baja y un retardo, teniendo en cuenta que simplemente son para supervisión pueden valer. Las imágenes obtenidas tendrán un aspecto similar al siguiente:

[[file:turtlebot_visualizacion.png|center]]

=Navegación autónoma=

==Calibración de odómetro y giroscopio==

Para un correcto seguimiento del movimiento del robot se debe hacer una calibración de su odómetro y su giroscopio, para corregir el error que puedan cometer. Para ello colocaremos el robot frente a una pared recta, con al menos 2 metros de superficie libre, separado unos 30 centímetros de ella, como se muestra en las imágenes.

[[file:turtlebot_calibracion.png|center]]

Para ejecutar el programa de calibración simplemente se debe ejecutar el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_calibration calibrate.launch</syntaxhighlight>

Finalizado el proceso de calibración aparecerán los valores de corrección de escala de los parámetros ''turtlebot_node/gyro_scale_correction'' y ''turtlebot_node/odom_angular_scale_correction''.

<syntaxhighlight>[INFO] [WallTime: 1299286750.821002] Multiply the 'turtlebot_node/gyro_scale_correction' parameter with VALOR_CALIBRACION
[INFO] [WallTime: 1299286750.822427] Multiply the 'turtlebot_node/odom_angular_scale_correction' parameter with VALOR_CALIBRACION</syntaxhighlight>

El nuevo valor a aplicar a los parámetros será el resultado de multiplicar el valor antiguo, por defecto 1, por el valor obtenido en la calibración. Para aplicar este nuevo valor a los parámetros de forma permanente se debe editar el archivo "turtlebot.launch", situado en la ruta '''/etc/ros/electric''', tal como se muestra a continuación:

<syntaxhighlight lang="xml" enclose="div">
<launch>



<param name="turtlebot_node/gyro_scale_correction" value="VALOR_ANTIGUO_x_VALOR_CALIBRACION"/>
<param name="turtlebot_node/odom_angular_scale_correction" value="VALOR_ANTIGUO_x_VALOR_CALIBRACION"/>

<include file="$(find turtlebot_bringup)/minimal.launch">
<arg name="urdf_file" value="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />
</include>

</launch>
</syntaxhighlight>

*'''Nota''': El parámetro gyro_measurement_range tiene por defecto un valor de 150, el valor adecuado si el [http://www.turtlebot.com Turtlebot] es de Willow Garage, pero debe ser 250 si es de [http://www.turtlebot.com Turtlebot] de Clearpath y 300 si es de iHeartEngineering (en este caso el giroscopio tiene un rango de medida de 300). Para esto será necesario añadir otra linea al archivo "turtlebot.launch" para establecer el valor adecuado según el modelo.

<syntaxhighlight><param name="turtlebot_node/gyro_measurement_range" value="VALOR_SEGÚN_MODELO"/></syntaxhighlight>

Para que el cambio de valores de los parámetros sea efectivo deberemos detener, y poner en marcha de nuevo el servicio del [http://www.turtlebot.com Turtlebot], con los siguientes comandos:

<syntaxhighlight>sudo service turtlebot stop
sudo service turtlebot start</syntaxhighlight>

*'''Nota''': También se puede hacer una modificación dinámica de estos parámetros usando un interfaz gráfico ejecutando el siguiente comando en un terminal:

<syntaxhighlight>rosrun dynamic_reconfigure reconfigure_gui</syntaxhighlight>

==Creación del mapa==

Para la creación del mapa se usará el stack proporcionado por [http://www.ros.org ROS] "slam_gmapping", que contiene el package "gmapping" que implementa un método para SLAM. El archivo "crear_mapa.launch", que se guardará en la carpeta "launch" de nuestro ''package'', tendrá el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />

<arg name="scan_topic" default="scan" />

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="odom"/>
<param name="map_update_interval" value="3.0"/>
<param name="maxUrange" value="16.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>

<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot_fer)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>

</syntaxhighlight>

*'''Nota''': los archivos de configuración del ''package'' "move_base" se han obtenido del ''package'' "turtlebot_navigation", carpeta "config", y se han situado en nuestro ''package'' en una carpeta llamada, igualmente, "config".
Para ejecutar "crear_mapa.launch", que inicia la Kinect, "slam_gmaping" y "move_base", se debe introducir el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_fer crear_mapa.launch</syntaxhighlight>

Para el desplazamiento del [http://www.turtlebot.com Turtlebot] a través del espacio a mapear se empleará el teleoperador por teclado, ejecutando en un terminal del PC robot el siguiente comando:

<syntaxhighlight>roslaunch turtlebot_teleop keyboard_teleop.launch</syntaxhighlight>

*'''Nota''': Para la correcta realización del mapa es necesario dar varias pasadas por el espacio a mapear, saliendo de un punto de partida y regresando a él para cerrar el bucle.
Para poder visualizar el proceso de creación del mapa, en un terminal del PC estación de trabajo se ejecutará el siguiente comando, que lanza el entorno de visualización "rviz" contenido en [http://www.ros.org ROS]:

<syntaxhighlight>rosrun rviz rviz -d `rospack find turtlebot_navigation`/nav_rviz.vcg</syntaxhighlight>

El resultado obtenido después de varias pasadas por el entorno a mapear es el siguiente (Laboratorio F6, robótica):

[[file:turtlebot_mapa.png|center]]

En el siguiente vídeo puede verse el el proceso de creación del mapa:
<center><videoflash>nfbD1vOdIU8</videoflash></center>

<center>[http://www.youtube.com/watch?v=nfbD1vOdIU8 Ver vídeo en YouTube]</center>

Se creará una carpeta llamada "mapas" dentro de nuestro ''package'', donde se guardará el mapa con el nombre "labf6" introduciendo el siguiente comando en un terminal del PC robot:

<syntaxhighlight>rosrun map_server map_saver -f /home/turtlebot/ros_workspace/turtlebot_fer/mapas/labf6</syntaxhighlight>

==Navegación por el entorno mapeado==

Con el mapa ya creado, para que el robot se desplace por él se hará uso del ''stack'' proporcionado por ROS "navigation", que contiene los ''packages'' "map_server", "amcl" (adaptive Monte Carlo localization) y “move_base”. Cada uno de ellos, respectivamente, carga el mapa creado, ejecuta el método probabilístico de localización adaptativa Monte Carlo y planifica la ruta para ir del origen al destino dentro del mapa evitando los obstaculos.
El ''package'' “move_base” se encarga de la planificación de rutas, navegación global, y evitación de obstáculos, navegación local. En la siguiente imagen se muestra el diagrama de bloques de la estructura de este ''package'':

[[file:turtlebot_move_base.png|center]]

Cuando el robot se encuentra atascado el procedimiento, por defecto, que sigue “move_base” para intentar recuperarlo es el que se muestra en la siguiente imagen, pudiendo ser modificado este comportamiento en el parámetro “recovery_behaviors”, por otro de los disponibles:

[[file:turtlebot_move_base_behabior.png|center|450px]]

Se creará, en la carpeta "launch" de nuestro ''package'', el archivo "usar_mapa.launch" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />

<arg name="map_file" default="$(find turtlebot_fer)/mapas/labf6.yaml"/>

<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan" />

<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>

<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>

<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>

<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>

<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot_fer)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>

</syntaxhighlight>

Para ejecutar "usar_mapa.launch", que inicia la Kinect, "map_server", "amcl" y "move_base", se debe introducir el siguiente comando en un terminal del PC robot:

roslaunch turtlebot_fer usar_mapa.launch map_file:=/home/turtlebot/ros_workspace/turtlebot_fer/mapas/labf6.yaml

Para interactuar con el robot se debe iniciar el entrono de visualización "rviz", ejecutando el siguiente comando en un terminal del PC estación de trabajo:

rosrun rviz rviz -d `rospack find turtlebot_navigation`/nav_rviz.vcg

Primero se deberá indicar la posición y orientación aproximada del robot dentro del mapa, pulsando en el botón "2D Pose Estimate" y posteriormente pulsando sobre el mapa en la posición estimada, sin soltar el botón del ratón indicar la orientación estimada y soltar. El modelo del robot se situará ahora en esta posición, rodeado por una nube de puntos en color rojo como se muestra en la imagen:

[[file:turtlebot_estimate.png|center]]

Para indicar el punto de destino dentro del mapa, pulsando "2D Nav Goal" y posteriormente pulsando sobre el mapa en la posición de destino, sin soltar el botón del ratón se indicara la orientación final. En este momento el robot comenzará a moverse siguiendo la trayectoria calculada, línea de color verde, hasta alcanzar el punto de destino, como se muestra en la imagen:

[[file:turtlebot_goal.png|center]]

En el siguiente vídeo puede verse el el proceso de indicación de la situación estimada del robot y navegación al destino marcado:

<center><videoflash>6CLc72Llfow</videoflash></center>

<center>[http://www.youtube.com/watch?v=6CLc72Llfow Ver vídeo en YouTube]</center>

En el siguiente vídeo se puede ver al [http://www.turtlebot.com Turtlebot] usando la navegación autónoma, y como esquiva un objeto no incluido en el mapa, después de ejecutar el comportamiento de recuperación tras un atascamiento, recalculando la ruta:

<center><videoflash>Sh4zswP1fVo</videoflash></center>

<center>[http://www.youtube.com/watch?v=Sh4zswP1fVo Ver vídeo en YouTube]</center>

*'''Nota''': Para poder probar la navegación autónoma en el [[#Simulador|simulador]] es necesario añadir la siguiente línea de código al archivo "robot.launch", situado en la ruta '''opt/ros/electric/stacks/turtlebot_simulator/turtlebot_gazebo/launch''':

<param name="output_frame" value="odom"/>

Esta línea deberá añadirse en la ejecución del ''node'' "robot_pose_ekf". Quedando el archivo "robot.launch" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />

<node name="spawn_turtlebot_model" pkg="gazebo" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf
-param robot_description -model turtlebot" respawn="false" output="screen"/>

<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
<rosparam command="load" file="$(find turtlebot_bringup)/config/diagnostics.yaml" />
</node>

<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="30.0" />
</node>



<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="false"/>
<param name="vo_used" value="false"/>
<param name="output_frame" value="odom"/> 
</node>


<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
<param name="max_rate" value="20.0"/>
<remap from="cloud_in" to="/camera/depth/points"/>
<remap from="cloud_out" to="cloud_throttled"/>
</node>


<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.15"/>
<param name="max_height" value="0.15"/>
<remap from="cloud" to="/cloud_throttled"/>
</node>


<node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.025"/>
<param name="max_height" value="0.025"/>
<remap from="cloud" to="/cloud_throttled"/>
<remap from="scan" to="/narrow_scan"/>
</node>

</launch>

</syntaxhighlight>

----
{| style="border: solid 0px white; width: 100%"
!Otros artículos
----
|-
| valign="top" width="100%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
|}
----

Fernando-TFM-ROS02

2014-11-05T16:32:10Z

Fernando:

* '''Project Name:''' Programas de control para el robot Turtlebot sobre ROS
* '''Authors:''' [http://www.fernando.casadogarcia.es Fernando Casado García]
* '''Academic Year:''' 2011-2012
* '''Degree:''' Master
* '''Tags:''' Turtlebot
* '''Technologies:''' ROS, c++
* '''Status:''' Finished

----
{| style="border: solid 0px white; width: 100%"
!Otros artículos
----
|-
| valign="top" width="100%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
|}
----

=Instalando ROS=

Lo primero que necesitamos es un computador con el sistema operativo [http://www.ubuntu.com Ubuntu], que es el único soportado por [http://www.ros.org ROS], o instalarlo; otros sistemas operativos para los que está disponible solo son experimentales. Para instalar [http://www.ubuntu.com Ubuntu] en un equipo con sistema operativo windows, podemos optar entre una instalación tradicional desde un CD-ROM o USB, en una partición diferente a la de windows, o la opción más rápida y sencilla , instalarlo desde windows como si se tratara de una aplicación o programa mas.

Provistos ya de un equipo con [http://www.ubuntu.com Ubuntu], para descargar e instalar el sistema [http://www.ros.org ROS] simplemente deberemos seguir las indicaciones de [http://www.ros.org/wiki/electric/Installation/Ubuntu instalación]. No esta de más el tomarse un tiempo realizando los [http://www.ros.org/wiki/ROS/Tutorials tutoriales] que proponen, son de gran ayuda para conocer la estructura de ficheros y el funcionamiento de ''Nodes'' y ''Topics''.

==Instalando ''Turtlebot-desktop''==

Con el sistema [http://www.ros.org ROS] instalado, necesitamos instalar las herramientas para desarrollo del robot [http://www.turtlebot.com Turtlebot] en el equipo de trabajo. Ejecutando el siguiente comando en un terminal comenzará la descarga e instalación:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-desktop</syntaxhighlight>

Es importante que el reloj del PC estación de trabajo se encuentre sincronizado con el reloj del PC robot, para prevenir las posibles perdidas de red inalámbrica. Para instalar el cliente [http://es.wikipedia.org/wiki/Network_Time_Protocol NTP] ''Chrony'' se ejecutará el siguiente comando en un terminal:

<syntaxhighlight>sudo apt-get install chrony</syntaxhighlight>

==Instalando ''Turtlebot-robot''==

Puede que se utilice otro equipo diferente al que suministran con el [http://www.turtlebot.com Turtlebot] o que se actualice [http://www.ubuntu.com Ubuntu] a una nueva versión, por lo que será necesario instalar la versión para el robor de [http://www.ros.org ROS]. Ejecutando el siguiente comando en un terminal comenzará la descarga e instalación:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-robot</syntaxhighlight>

Se necesita crear un archivo llamado “52-turtlebot.rules” en el directorio '''/etc/udev/rules.d/'''. Con el siguiente contenido:

<syntaxhighlight>ATTRS{idProduct}=="XXXX",ATTRS{idVendor}=="XXXX",MODE="666",GROUP="turtlebot"</syntaxhighlight>

Donde las “XXXX” corresponden a la identificación del producto y del fabricante, respectivamente, del adaptador USB de conexión con el iRobot Create. Esta información la obtiene ejecutando el siguiente comando en un terminal, con el adaptador conectado:

<syntaxhighlight>lsusb</syntaxhighlight>

Que mostrará una información similar a esta:
<syntaxhighlight>
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 002 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 003 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 004 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 005 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 002 Device 002: ID 0a5c:2101 Broadcom Corp. Bluetooth Controller
Bus 003 Device 002: ID 04fc:0801 Sunplus Technology Co., Ltd
Bus 004 Device 002: ID 08ff:2580 AuthenTec, Inc. AES2501 Fingerprint Sensor
Bus 004 Device 003: ID 0403:6001 Future Technology Devices International, Ltd FT232 USB-Serial (UART) IC
</syntaxhighlight>

Donde “6001” corresponde a la identificación del producto y “0403” a la identificación del fabricante.
También se debe añadir el usuario, con el que hemos instalado ROS, al grupo “dialout”, para que tenga permisos sobre el puerto que se emplea para la comunicación con el iRobot Create, ttyUSB0.

=Conociendo el Turtlebot=

[[File:Turtlebot.png|thumb|130px|Fotografía del [http://www.turtlebot.com Turtlebot].]]

==Hardware==

* [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create]
* Laptop ASUS 1215N
* Microsoft Kinect
* Placa de gestión de alimentación y giroscopio
* Cables USB de comunicaciones y alimentación
* Estructura de soporte de elementos

==Software==

* Sistema operativo [http://www.ubuntu.com Ubuntu]
* [http://www.ros.org ROS] con ''Turtlebot-robot''

=Conexión estación de trabajo-robot=

==Configuración de la red==

Lo habitual es conectar ambos equipos a una red [http://es.wikipedia.org/wiki/Wi-Fi Wi-Fi], pero puede darse que el PC estación de trabajo no disponga de un interfaz de red inalámbrica. En este caso es necesario poder conectarse a un puerto del [http://es.wikipedia.org/wiki/Router router] que provee la red [http://es.wikipedia.org/wiki/Wi-Fi Wi-Fi], para que exista una ruta de acceso entre el PC estación de trabajo y el PC robot.

En ambos equipos, PC estación de trabajo y PC robot, se añadirá el ROS_MASTER_URI en el archivo de configuración del terminal (~/.bashrc), que apunta a la dirección IP del PC robot, que es el que ejecuta el servicio maestro. Simplemente se debe escribir el siguiente comando en un terminal, sustituyendo "IP_OF_TURTLEBOT", por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>echo export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311 >> ~/.bashrc</syntaxhighlight>

También se debe asignar un nombre de equipo, tanto al PC estación de trabajo como al PC robot. Este nombre será la dirección IP de cada equipo, que se almacenará en el archivo de configuración del terminal (~/.bashrc). En un terminal del PC robot se ejecutará el siguiente comando, sustituyendo "IP_OF_TURTLEBOT", por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>echo export ROS_HOSTNAME=IP_OF_TURTLEBOT >> ~/.bashrc</syntaxhighlight>

En un terminal del PC estación de trabajo se ejecutará el siguiente comando, sustituyendo "IP_OF_WORKSTATION", por la dirección IP asignada en la red al PC estación de trabajo:

<syntaxhighlight>echo export ROS_HOSTNAME=IP_OF_WORKSTATION >> ~/.bashrc</syntaxhighlight>

* '''Nota''': Para conocer la dirección IP, asignada en la red a un equipo, simplemente se debe ejecutar en un terminal:

<syntaxhighlight>ifconfig</syntaxhighlight>

que mostrará una información similar a esta:

<syntaxhighlight>
eth0 Link encap:Ethernet direcciónHW 00:16:d4:55:85:b4
ACTIVO DIFUSIÓN MULTICAST MTU:1500 Métrica:1
Paquetes RX:0 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:0 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:1000
Bytes RX:0 (0.0 B) TX bytes:0 (0.0 B)
Interrupción:44

lo Link encap:Bucle local
Direc. inet:127.0.0.1 Másc:255.0.0.0
Dirección inet6: ::1/128 Alcance:Anfitrión
ACTIVO BUCLE FUNCIONANDO MTU:16436 Métrica:1
Paquetes RX:64 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:64 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:0
Bytes RX:5184 (5.1 KB) TX bytes:5184 (5.1 KB)

wlan0 Link encap:Ethernet direcciónHW 00:18:de:21:4b:ad
Direc. inet:192.168.1.10 Difus.:192.168.1.255 Másc:255.255.255.0
Dirección inet6: fe80::218:deff:fe21:4bad/64 Alcance:Enlace
ACTIVO DIFUSIÓN FUNCIONANDO MULTICAST MTU:1500 Métrica:1
Paquetes RX:130972 errores:0 perdidos:0 overruns:0 frame:0
Paquetes TX:87840 errores:0 perdidos:0 overruns:0 carrier:0
colisiones:0 long.colaTX:1000
Bytes RX:169415208 (169.4 MB) TX bytes:9379691 (9.3 MB)
</syntaxhighlight>

==Puesta en marcha==

Con la red configurada, ya se puede proceder a la puesta en marcha del [http://www.turtlebot.com Turtlebot]. Se conectarán los cables USB de comunicación y alimentación del robot al PC robot, se encenderá el [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create] y se arrancará el PC robot.

Las operaciones sobre el PC robot se pueden hacer directamente sobre él, pero no es la opción recomendable por comodidad de trabajo. Así que se comenzará conectando por [http://es.wikipedia.org/wiki/Secure_Shell ssh], desde el PC estación de trabajo con el PC robot. Se ejecutará el siguiente comando en un terminal en la estación de trabajo, sustituyendo "IP_OF_TURTLEBOT" por la dirección IP asignada en la red al PC robot:

<syntaxhighlight>ssh turtlebot@IP_OF_TURTLEBOT</syntaxhighlight>

Pedirá la contraseña asignada al PC robot. Cuando se conecte el [http://es.wikipedia.org/wiki/Prompt prompt] del terminal cambiará por el del PC robot. Se puede comprobar el estado del servicio del robot, tecleando el siguiente comando en el terminal:

<syntaxhighlight>sudo service turtlebot status</syntaxhighlight>

Devolverá un mensaje diciendo que está iniciado. El servicio se inicia siempre que encendemos el PC robot, pero se puede detener y poner en marcha desde el terminal con los siguientes comandos:

<syntaxhighlight>sudo service turtlebot stop
sudo service turtlebot start</syntaxhighlight>

En un terminal nuevo, de forma local sin conectar con el PC robot, se ejecutará la aplicación del tablero de instrumentos (dashboard) del robot. Simplemente se debe introducir este comando en el terminal:

<syntaxhighlight>rosrun turtlebot_dashboard turtlebot_dashboard&</syntaxhighlight>

Aparecerá una pequeña ventana similar a esta:

[[file:turtlebot_dashboard_0.png|center]]

La ventana del tablero de instrumentos consta de tres zonas bien diferenciadas:
* Diagnóstico del sistema, mensajes del sistema y modo de operación.
* Interruptores de los circuitos de las tres salidas digitales del [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create].
* Niveles de batería de el [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create] y el PC robot.
Si todo va bien, como en la imagen, estarán en verde los iconos de diagnostico del sistema (llave inglesa) y mensajes del sistema (nube de llamada). El modo de operación se debe poner poner en ''Full Mode'', para poder operar el robot. En los otros modos no está permitida la operación del robot, son el modo de carga (Passive Mode) y de espera (Safety Mode).

[[file:turtlebot_dashboard.png|center]]

También se debe activar al menos el interruptor "1", que es el que controla el circuito de alimentación de la cámara Kinect. Ahora ya esta todo listo para comenzar a operar con el robot. Para verlo en funcionamiento es recomendable probar alguna de las [http://www.ros.org/wiki/turtlebot_apps aplicaciones] de demostración.

=Primer programa (Vuelta al mundo)=

==Creando un espacio de trabajo==

Lo más cómodo para trabajar es crear un directorio propio para ''stacks'' y ''packages'' en el PC estación de trabajo, ya que [http://www.ros.org ROS] tiene por defecto como directorio para ''stacks'' y ''packages'' '''/opt/ros/electric/stacks'''. Se pueden crear los nuevos ''stacks'' o ''packages'' en este directorio por defecto, pero queda más organizado y accesible de esta forma.

El directorio se puede situar donde y con el nombre que se quiera. Se comenzará creando un directorio, por ejemplo en la carpeta personal:

<syntaxhighlight>mkdir ~/ros_workspace</syntaxhighlight>

Para que el sistema también busque ''stacks'' y ''packages'' en nuestra carpeta, se debe crear en la carpeta personal un archivo llamado "setup.sh", con el siguiente contenido:
<syntaxhighlight>
#!/bin/sh
source /opt/ros/electric/setup.bash
export ROS_ROOT=/opt/ros/electric/ros
export PATH=$ROS_ROOT/bin:$PATH
export PYTHONPATH=$ROS_ROOT/core/roslib/src:$PYTHONPATH
export ROS_PACKAGE_PATH=~/ros_workspace:/opt/ros/electric/stacks:$ROS_PACKAGE_PATH
</syntaxhighlight>

Se debe añadir este nuevo archivo creado al archivo de configuración del terminal (~/.bashrc), para hacer efectiva esta modificación. Para ello se debe ejecutar el siguiente comando en un terminal:

<syntaxhighlight>echo source ~/setup.sh >> ~/.bashrc</syntaxhighlight>

==Creando un ''package''==

En el directorio de trabajo creado se creará un ''package'', el cual va a depender de otros ''packages'' del sistema. Las dependencias van en función del tipo de mensajes que queremos enviar o recibir, y el lenguaje empleado en los programas. En este primer programa no se necesitan todas las dependencias, pero se usaran en los siguientes programas, por lo que las añadiremos al crear el ''package'' que va a contener los programas. Para esto se introducirá la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>cd ~/ros_workspace
roscreate-pkg turtlebot_fer std_msgs turtlebot_node geometry_msgs nav_msgs sensor_msgs image_transport roscpp</syntaxhighlight>

También se pueden añadir dependencias a un ''package'' ya creado, simplemente se debe editar el archivo "manifest.xml" que se encuentra dentro de la carpeta del ''package'' creado. El contenido del archivo "manifest.xml" es similar al siguiente, las dependencias se encuentran al final:

<syntaxhighlight lang="xml" enclose="div">
<package>
<description brief="turtlebot_fer">

turtlebot_fer

</description>
<author>FeR</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/turtlebot_fer</url>
<depend package="turtlebot_node"/>
<depend package="geometry_msgs"/>
<depend package="nav_msgs"/>
<depend package="std_msgs"/>
<depend package="sensor_msgs"/>
<depend package="image_transport"/>
<depend package="roscpp"/>

</package>

</syntaxhighlight>

Una vez creado el ''package'', o modificadas las dependencias de un ''package'' existente, se debe compilar para que se hagan efectivas las dependencias. Simplemente se debe ejecutar el siguiente comando en el terminal, indicando el nombre del ''package'':

<syntaxhighlight>rosmake turtlebot_fer</syntaxhighlight>

==Escribiendo y compilando el programa==

Los programas se pueden escribir en c++ o python, el escogido ha sido c++. Para la redacción del programa se puede emplear algún editor de código, como [http://www.geany.org/ Geany]. Los archivos de los programas se guardarán en la carpeta "src" del ''package'' creado, turtlebot_fer. Este primer programa, simplemente crea el ''nodo'' "gira" y publica el ''topic'' "cmd_vel", que hace mover el robot con una velocidad de avance y de giro, describiendo un círculo, hasta que se reciba la señal de parada "Ctrl+C". El programa es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"

int main(int argc, char **argv)
{

ros::init(argc, argv, "gira"); //Creación del nodo "gira"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

geometry_msgs::Twist vel;

ros::Rate loop_rate(10); //Frecuencia de realización del bucle (10 Hz)

while (ros::ok()) //Bucle mientras no se reciba "Ctrl+C"
{
vel.linear.x = 0.2; //velocidad de avance
vel.angular.z = 0.4; //velocidad de giro

vel_pub_.publish(vel);

loop_rate.sleep();
}

return 0;
}

</syntaxhighlight>

Se guardará con el nombre "gira.cpp". Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight>rosbuild_add_executable(gira src/gira.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd turtlebot_fer
make</syntaxhighlight>

==Ejecutando el programa==

===Simulador===

Lo primero que hay que hacer es descargar el simulador del [http://www.turtlebot.com Turtlebot], ejecutando el siguiente comando en unterminal:

<syntaxhighlight>sudo apt-get install ros-electric-turtlebot-simulator</syntaxhighlight>

Se puede ejecutar primero en el simulador para salvaguardar la integridad del robot. Lo primero que se debe hacer para poder arrancar el simulador es comentar las líneas que se han añadido para la comunicación con el PC robot en el archivo "~/.bashrc", del PC estación de trabajo, tal y como se muestra, añadiendo el símbolo "#" al principio de la línea:

<syntaxhighlight># export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311
# export ROS_HOSTNAME=IP_OF_WORKSTATION</syntaxhighlight>

Se debe reiniciar la sesión de usuario para que surtan efecto los cambios. Para ejecutar el simulador del [http://www.turtlebot.com Turtlebot] simplemente se bebe ejecutar el siguiente comando en un terminal:

<syntaxhighlight>roslaunch turtlebot_gazebo turtlebot_empty_world.launch</syntaxhighlight>

Terminada la carga aparecerá una ventana gráfica del simulador [http://gazebosim.org/ Gazebo] con un escenario vacío y el modelo 3D del robot, como se muestra en la imagen:

[[file:turtlebot_sim.png|center]]

Se puede cambiar el punto de vista jugando con la rueda del ratón y el botón derecho. Además, se pueden añadir objetos 3D, como esferas, prismas y cilindros. Para ejecutar el programa creado ejecutamos el siguiente comando en un terminal nuevo, indicando el ''package'' donde se encuentra y el nombre del ejecutable:

<syntaxhighlight>rosrun turtlebot_fer gira</syntaxhighlight>

Si todo ha ido bien, el modelo 3D del robot comenzara a desplazarse formando un circulo, y no se detendrá hasta que reciba la señal de parada el terminal desde el que se ejecuta el programa, "Ctrl+C".

===Robot===

Si se han modificado las lineas para comunicación del archivo "~/.bashrc", para ejecutar el simulador, se deben restaurar y reiniciar la sesión de usuario para que surtan efecto los cambios. Ahora se procederá a la [[#Puesta en marcha|puesta en marcha]] del robot.

Conectado el PC estación de trabajo al PC robot, se debe [[#Creando un espacio de trabajo|crear un espacio de trabajo]] y [[#Creando un package|crear un ''package'']], al igual que en el PC estación de trabajo. Creado el espacio de trabajo y el ''package'', se copiará el programa situado en el PC estación de trabajo al PC robot, escribiendo el siguiente comando en un terminal del PC estación de trabajo:

<syntaxhighlight>roscd turtlebot_fer
cd src
scp gira.cpp turtlebot@IP_OF_TURTLEBOT:/home/turtlebot/ros_workspace/turtlebot_fer/src/gira.cpp</syntaxhighlight>

Una vez copiado el archivo del programa al PC robot, para poder ser ejecutado hay que [[#Escribiendo y compilando el programa|compilarlo]] desde un terminal del PC robot. Para ejecutar el programa simplemente se debe introducir el siguiente comando en un terminal del PC robot, tomando la precaución que el robot se encuentre en un espacio lo bastante diáfano para evitar colisiones:

<syntaxhighlight>rosrun turtlebot_fer gira</syntaxhighlight>

En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>ZY1oZWB7Vn4</videoflash></center>

<center>[http://www.youtube.com/watch?v=ZY1oZWB7Vn4 Ver vídeo en YouTube]</center>

* '''Nota''': si se encuentra conectada al robot alguna de las fuentes de alimentación, PC robot o [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create], este no se moverá.

=Segundo programa (Golpea y escapa)=

Este segundo programa añade la suscripción a un ''topic'', además de la publicación. El funcionamiento es bastante simple, comenzará a avanzar en línea recta hasta que se detecte el accionamiento del parachoques del [http://store.irobot.com/shop/index.jsp?categoryId=3311368 iRobot Create], provocado por el choque con un objeto; en este momento realizará un pequeño retroceso y girará un ángulo, de sentido y magnitud aleatorios, para intentar evitar el obstáculo. No se detendrá hasta recibir la señal de parada "Ctrl+C". El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"
#include "turtlebot_node/TurtlebotSensorState.h" //Librería "TurtlebotSensorState" del package "turtlebot_node"

geometry_msgs::Twist vel; //Declaración de la variable global vel
double alea; //Declaración de la variable global alea

void callback(const turtlebot_node::TurtlebotSensorState& sen) //Función de llamda con cada dato recibido
{
ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Rate loop_rate(10); //Frecuencia de realización del bucle (10 Hz)

ros::Time ahora; //Declaración de variable tipo tiempo, variable ROS

if (sen.bumps_wheeldrops == 0) //Comprobación si ha habido señal del parachoques
{
::vel.linear.x = 0.2;
::vel.angular.z = 0.0;

vel_pub_.publish(::vel);
}
else
{
ahora = ros::Time::now(); //Asignación del tiempo actual a la variable ahora

while (ros::Time::now() < (ahora + ros::Duration(0.5))) //Bucle durante 0,5 segundos, retroceso después de choque
{
::vel.linear.x = -0.2;
::vel.angular.z = 0.0;

vel_pub_.publish(::vel);

loop_rate.sleep();
}

::alea = ((rand() %40) -20)/10; //Asignación de valor aleatorio

while (::alea == 0) //Bucle mientras la variable aleatoria sea cero
{
::alea = ((rand() %40) -20)/10;
}

ahora = ros::Time::now(); //Asignación del tiempo actual a la variable ahora

while (ros::Time::now() < (ahora + ros::Duration(1.5))) //Bucle durante 1,5 segundos, giro de ángulo aleatorio
{
::vel.linear.x = 0.0;
::vel.angular.z = ::alea;

vel_pub_.publish(::vel);

loop_rate.sleep();
}
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "juguete_golpe"); //Creación del nodo "juguete_golpe"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Subscriber odom_sub_= n.subscribe("turtlebot_node/sensor_state", 1, callback); //Suscripción del topic "turtlebot_node/sensor_state"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "juguete_golpe.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]].
En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>w7k0381Nfb8</videoflash></center>

<center>[http://www.youtube.com/watch?v=w7k0381Nfb8 Ver vídeo en YouTube]</center>

* '''Nota''': El modelo 3D del [http://www.turtlebot.com Turtlebot] empleado en el simulador no implementa este sensor, por lo que no será posible probar el programa en el [[#Simulador|simulador]].

=Tercer programa (Toro loco)=

La peculiaridad de este programa es que el ''topic'' de suscripción es la imagen obtenida por la cámara Kinect. La imagen se recibe empaquetada en un array de "píxeles ancho x píxeles alto x 3 (componentes color rgb)" elementos, por defecto (640 x 480 x 3) = 921600 elementos. Los píxeles se encuentran representados en sus componentes rgb, con ese orden, comenzando en el píxel de la esquina superior izquierda, continuando por filas hasta llegar al último píxel, esquina inferior derecha de la imagen.

{| class="wikitable" align="center"
! align = "center" colspan = "3" |Píxel 1 !! align = "center" colspan = "3" |Píxel 2 !! align = "center" colspan = "3" |Píxel 3 !! align = "center" | !! align = "center" colspan = "3" |Píxel 307200
|-
|bgcolor = "red" align = "center" width="40px"| 100 ||bgcolor = "green" align = "center" width="40px" | 65 ||bgcolor = "blue" align = "center" width="40px" | 50 ||bgcolor = "red" align = "center" width="40px" | 110 ||bgcolor = "green" align = "center" width="40px"| 63 ||bgcolor = "blue" align = "center" width="40px"| 49 ||bgcolor = "red" align = "center" width="40px"| 109 ||bgcolor = "green" align = "center" width="40px"| 65 ||bgcolor = "blue" align = "center" width="40px"| 48 ||align = "center" width="40px"| ... ||bgcolor = "red" align = "center" width="40px"| 75 ||bgcolor = "green" align = "center" width="40px"| 98 ||bgcolor = "blue" align = "center" width="40px"| 244
|-
|align = "center"|[0] ||align = "center"|[1] ||align = "center"|[2] ||align = "center"|[3] ||align = "center"|[4] ||align = "center"|[5] ||align = "center"|[6] ||align = "center"|[7] ||align = "center"|[8] ||align = "center"| ||align = "center"|[921597] ||align = "center"|[921598] ||align = "center"|[921599]
|}

Este tercer programa realiza el seguimiento de objetos de color rojo. Para ello analiza cada imagen píxel a píxel, contando los píxeles del lado izquierdo y del lado derecho. Cuando el número total de píxeles de tonalidades rojas es superior al 0,1 %, de los píxeles totales de la imagen, e inferior al 5 %, comienza el seguimiento hasta hacer el numero de píxeles totales cercano al 2,5 % e igual a cada lado (estos valores dependerán del tamaño del objeto). Cuando el objeto se encuentre demasiado lejos o demasiado cerca se detendrá el seguimiento. Para facilitar la calibración cada vez que se quiera modificar alguno de los parámetros del programa se creará un archivo .h, en el que estarán definidas las constantes para cada parámetro, se guardará con el nombre "param_sigue_rojo.h" y con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#ifndef __PARAM_SIGUE_ROJO_H
#define __PARAM_SIGUE_ROJO_H

#define li_red 100 //Límite inferior para canal rojo
#define ls_red 255 //Límite superior para canal rojo
#define li_green 0 //Límite inferior para canal verde
#define ls_green 30 //Límite superior para canal verde
#define li_blue 0 //Límite inferior para canal azul
#define ls_blue 50 //Límite superior para canal azul

#define tam_min 0.001 //Porcentaje de píxeles de tonalidades rojas mínimo
#define tam_max 0.05 //Porcentaje de píxeles de tonalidades rojas máximo
#define tam_med 0.025 //Porcentaje de píxeles de tonalidades rojas medio

#define Px 0.08 //Ganancia velocidad de avance-retroceso
#define Pz 1.2 //Ganancia velocidad de giro

#endif /*__PARAM_SIGUE_ROJO_H*/

</syntaxhighlight>

El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "sensor_msgs/Image.h" //Librería "Image" del package "sensor_msgs"
#include "geometry_msgs/Twist.h" //Librería "Twist" del package "geometry_msgs"
#include "param_sigue_rojo.h" //Parámetros de ajuste

geometry_msgs::Twist vel; //Declaración de la variable global vel
sensor_msgs::Image region; //Declaración de la variable global vel

void callback(const sensor_msgs::Image& ima)
{
ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

uint pix_r; //Declaración de variables de almacenaje valores rgb de cada píxel
uint pix_g;
uint pix_b;
uint ancho = ima.step; //Número de elementos de cada fila de la imagen
uint tam = ima.data.size(); //Número de elementos del array

double cont_izq = 0; //Declaración e inicialización de contadores de píxeles
double cont_der = 0;

::region = ima;
uint up;
uint right;
uint down;
uint left;

for (uint i = 0; i < tam; i = i + ancho) //Filtro de regiones pequeñas o píxeles aislados
{
for (uint j = 0; j < ancho; j = j + 3)
{
pix_r = ima.data[i + j];
pix_g = ima.data[i + j + 1];
pix_b = ima.data[i + j + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
up = 12;
if ((i/ancho) < 12)
{
up = (i/ancho);
}
right = 12;
if (((ancho - j)/3) < 12)
{
right = ((ancho - j)/3);
}
down = 12;
if (((tam/ancho) - (i/ancho)) < 12)
{
down = ((tam/ancho) - (i/ancho));
}
left = 12;
if ((j/3) < 12)
{
left = (j/3);
}

uint contr = 0;

for (uint k = i + j - (ancho*up) - (3*left); k <= (i + j + (ancho*down) - (3*left)); k = k + ancho)
{
for(uint l = 0; l < ((3*left) + (3*right) +3); l = l + 3)
{
pix_r = ima.data[k + l];
pix_g = ima.data[k + l + 1];
pix_b = ima.data[k + l + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
contr++;
}
}
}
if (contr > (625*0.5))
{
::region.data[i + j] = 1;
::region.data[i + j + 1] = 1;
::region.data[i + j + 2] = 1;
}
}
}
}

for (uint i = 0; i < (tam - (ancho/2)); i = i + ancho) //Contar píxeles tonos rojos lado izquierdo imagen
{
for (uint j = 0; j < (ancho/2); j = j + 3)
{
pix_r = ::region.data[i + j];
pix_g = ::region.data[i + j + 1];
pix_b = ::region.data[i + j + 2];

if (pix_r == 1 && pix_g == 1 && pix_b == 1)
{
cont_izq++;
}
}
}

for (uint i = (ancho/2); i < tam; i = i + ancho) //Contar píxeles tonos rojos lado derecho imagen
{
for (uint j = 0; j < (ancho/2); j = j + 3)
{
pix_r = ::region.data[i + j];
pix_g = ::region.data[i + j + 1];
pix_b = ::region.data[i + j + 2];

if (pix_r == 1 && pix_g == 1 && pix_b == 1)
{
cont_der++;
}
}
}

double cont = cont_izq + cont_der;

if (cont > ((tam/3) * tam_min) && cont < ((tam/3) * tam_max)) //Comprobación si se está dentro del limite inferior y superior de píxeles
{
::vel.angular.z = (cont_izq - cont_der) / (::abs(cont_izq - cont_der) * Pz); //Velocidad de giro
::vel.linear.x = (((tam/3) * tam_med) - cont) / ((tam/3) * Px); //Velocidad de avance-retroceso
vel_pub_.publish(vel);
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "sigue_rojo"); //Creación del nodo "sigue_rojo"

ros::NodeHandle n;

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //Publicación del topic "cmd_vel"

ros::Subscriber image_sub_= n.subscribe("camera/rgb/image_color", 1, callback); //Suscripción del topic "camera/rgb/image_color"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

* '''Nota''': Ya que la tonalidad percibida por la cámara depende de la iluminación del recinto, es necesaria una calibración de los límites de los canales RGB, para un correcto funcionamiento.

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "sigue_rojo.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]]. Para poner en marcha la cámara Kinect se debe lanzar en el PC robot el "kinect.launch", ejecutando el siguiente comando en un terminal:

<syntaxhighlight>roslaunch turtlebot_bringup kinect.launch</syntaxhighlight>

En el siguiente vídeo puede verse el robot ejecutando el programa:
<center><videoflash>LSXP9Xgl1MI</videoflash></center>

<center>[http://www.youtube.com/watch?v=LSXP9Xgl1MI Ver vídeo en YouTube]</center>

Además para la supervisión de las imagenes procesadas por el programa se ha realizado otro programa que publica el ''topic'' "/camara/rojo" con la imagen modificada, en la que los píxeles de tonalidad roja se colorean en rojo y el resto se muestra en escala de grises, con una linea divisoria de color negro en el centro. El código del programa, escrito en lenguaje c++, es el siguiente:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h" //Librería de funciones del sistema ROS
#include "sensor_msgs/Image.h" //Librería "Image" del package "sensor_msgs"
#include "param_sigue_rojo.h" //Parámetros de ajuste

sensor_msgs::Image region; //Declaración de la variable global region

void callback(const sensor_msgs::Image& ima)
{
ros::NodeHandle n;

ros::Publisher rojo_pub_=n.advertise<sensor_msgs::Image>("camara/rojo", 1); //Publicación del topic "camara/rojo"

uint pix_r; //Declaración de variables de almacenaje valores rgb de cada píxel
uint pix_g;
uint pix_b;
uint ancho = ima.step; //Número de elementos de cada fila de la imagen
uint tam = ima.data.size(); //Número de elementos del array

::region = ima;
uint up;
uint right;
uint down;
uint left;

for (uint i = 0; i < tam; i = i + ancho) //Filtro de regiones pequeñas o píxeles aislados
{
for (uint j = 0; j < ancho; j = j + 3)
{
pix_r = ima.data[i + j];
pix_g = ima.data[i + j + 1];
pix_b = ima.data[i + j + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
up = 12;
if ((i/ancho) < 12)
{
up = (i/ancho);
}
right = 12;
if (((ancho - j)/3) < 12)
{
right = ((ancho - j)/3);
}
down = 12;
if (((tam/ancho) - (i/ancho)) < 12)
{
down = ((tam/ancho) - (i/ancho));
}
left = 12;
if ((j/3) < 12)
{
left = (j/3);
}

uint contr = 0;

for (uint k = i + j - (ancho*up) - (3*left); k <= (i + j + (ancho*down) - (3*left)); k = k + ancho)
{
for(uint l = 0; l < ((3*left) + (3*right) +3); l = l + 3)
{
pix_r = ima.data[k + l];
pix_g = ima.data[k + l + 1];
pix_b = ima.data[k + l + 2];

if (pix_r >= li_red && pix_r <= ls_red && pix_g >= li_green && pix_g <= ls_green && pix_b >= li_blue && pix_b <= ls_blue)
{
contr++;
}
}
}
if (contr > (625*0.5))
{
::region.data[i + j] = 255;
::region.data[i + j + 1] = 0;
::region.data[i + j + 2] = 0;

}
else //Conversión a escala de grises regiones pequeñas o píxeles aislados
{
if (pix_r > pix_g)
{
::region.data[i + j] = pix_r;
::region.data[i + j + 1] = pix_r;
::region.data[i + j + 2] = pix_r;
}
else if (pix_g > pix_b)
{
::region.data[i + j] = pix_g;
::region.data[i + j + 1] = pix_g;
::region.data[i + j + 2] = pix_g;
}
else
{
::region.data[i + j] = pix_b;
::region.data[i + j + 1] = pix_b;
::region.data[i + j + 2] = pix_b;
}
}
}
else //Conversión a escala de grises resto píxeles
{
if (pix_r > pix_g)
{
::region.data[i + j] = pix_r;
::region.data[i + j + 1] = pix_r;
::region.data[i + j + 2] = pix_r;
}
else if (pix_g > pix_b)
{
::region.data[i + j] = pix_g;
::region.data[i + j + 1] = pix_g;
::region.data[i + j + 2] = pix_g;
}
else
{
::region.data[i + j] = pix_b;
::region.data[i + j + 1] = pix_b;
::region.data[i + j + 2] = pix_b;
}
}
}
}

for (uint i = (ancho/2)-3; i < tam; i = i + ancho) //Línea negra divisoria central
{
for (uint j = 0; j < 6; j = j + 3)
{
::region.data[i + j] = 0;
::region.data[i + j + 1] = 0;
::region.data[i + j + 2] = 0;
}
}

rojo_pub_.publish(::region);
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "visualizacion"); //Creación del nodo "visualizacion"

ros::NodeHandle n;

ros::Publisher rojo_pub_=n.advertise<sensor_msgs::Image>("camara/rojo", 1); //Publicación del topic "camara/rojo"

ros::Subscriber image_sub_= n.subscribe("camera/rgb/image_color", 1, callback); //Suscripción del topic "camera/rgb/image_color"

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"

return 0;
}

</syntaxhighlight>

Si se tiene ya creado el [[#Creando un espacio de trabajo|espacio de trabajo]] y el [[#Creando un package|''package'']], simplemente se debe guardar en un archivo llamado "visualizacion.cpp", [[#Escribiendo y compilando el programa|compilar]] y [[#Robot|ejecutar]].

Para poder ejecutar "kinect.launch", "sigue_rojo" y "visualización"de forma simultanea se creará un ''launcher''. Para ello crearemos dentro de la carpeta de nuestro ''package'' una carpeta llamada "launch", dentro de la cual crearemos un archivo llamado "rojo.launch" con el siguiente contenido:
<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />
<node name="sigue_rojo" pkg="turtlebot_fer" type="sigue_rojo" />
<node name="visualizacion" pkg="turtlebot_fer" type="visualizacion" />
</launch>
</syntaxhighlight>

Para ejecutar el ''launcher'' que lanza los dos nodos, "sigue_rojo" y "visualización", simplemente habrá que ejecutar el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_fer rojo.launch</syntaxhighlight>

Para poder visualizar la imagen modificada de la cámara Kinect en el PC estación de trabajo, para la supervisión. Simplemente hay que ejecutar el siguiente comando en un terminal:

<syntaxhighlight>rosrun image_view image_view image:=/camara/rojo</syntaxhighlight>

Las imágenes tendrán una tasa de frames baja y un retardo, teniendo en cuenta que simplemente son para supervisión pueden valer. Las imágenes obtenidas tendrán un aspecto similar al siguiente:

[[file:turtlebot_visualizacion.png|center]]

=Navegación autónoma=

==Calibración de odómetro y giroscopio=

Para un correcto seguimiento del movimiento del robot se debe hacer una calibración de su odómetro y su giroscopio, para corregir el error que puedan cometer. Para ello colocaremos el robot frente a una pared recta, con al menos 2 metros de superficie libre, separado unos 30 centímetros de ella, como se muestra en las imágenes.

[[file:turtlebot_calibracion.png|center]]

Para ejecutar el programa de calibración simplemente se debe ejecutar el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_calibration calibrate.launch</syntaxhighlight>

Finalizado el proceso de calibración aparecerán los valores de corrección de escala de los parámetros ''turtlebot_node/gyro_scale_correction'' y ''turtlebot_node/odom_angular_scale_correction''.

<syntaxhighlight>[INFO] [WallTime: 1299286750.821002] Multiply the 'turtlebot_node/gyro_scale_correction' parameter with VALOR_CALIBRACION
[INFO] [WallTime: 1299286750.822427] Multiply the 'turtlebot_node/odom_angular_scale_correction' parameter with VALOR_CALIBRACION</syntaxhighlight>

El nuevo valor a aplicar a los parámetros será el resultado de multiplicar el valor antiguo, por defecto 1, por el valor obtenido en la calibración. Para aplicar este nuevo valor a los parámetros de forma permanente se debe editar el archivo "turtlebot.launch", situado en la ruta '''/etc/ros/electric''', tal como se muestra a continuación:

<syntaxhighlight lang="xml" enclose="div">
<launch>



<param name="turtlebot_node/gyro_scale_correction" value="VALOR_ANTIGUO_x_VALOR_CALIBRACION"/>
<param name="turtlebot_node/odom_angular_scale_correction" value="VALOR_ANTIGUO_x_VALOR_CALIBRACION"/>

<include file="$(find turtlebot_bringup)/minimal.launch">
<arg name="urdf_file" value="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />
</include>

</launch>
</syntaxhighlight>

*'''Nota''': El parámetro gyro_measurement_range tiene por defecto un valor de 150, el valor adecuado si el [http://www.turtlebot.com Turtlebot] es de Willow Garage, pero debe ser 250 si es de [http://www.turtlebot.com Turtlebot] de Clearpath y 300 si es de iHeartEngineering (en este caso el giroscopio tiene un rango de medida de 300). Para esto será necesario añadir otra linea al archivo "turtlebot.launch" para establecer el valor adecuado según el modelo.

<syntaxhighlight><param name="turtlebot_node/gyro_measurement_range" value="VALOR_SEGÚN_MODELO"/></syntaxhighlight>

Para que el cambio de valores de los parámetros sea efectivo deberemos detener, y poner en marcha de nuevo el servicio del [http://www.turtlebot.com Turtlebot], con los siguientes comandos:

<syntaxhighlight>sudo service turtlebot stop
sudo service turtlebot start</syntaxhighlight>

*'''Nota''': También se puede hacer una modificación dinámica de estos parámetros usando un interfaz gráfico ejecutando el siguiente comando en un terminal:

<syntaxhighlight>rosrun dynamic_reconfigure reconfigure_gui</syntaxhighlight>

==Creación del mapa==

Para la creación del mapa se usará el stack proporcionado por [http://www.ros.org ROS] "slam_gmapping", que contiene el package "gmapping" que implementa un método para SLAM. El archivo "crear_mapa.launch", que se guardará en la carpeta "launch" de nuestro ''package'', tendrá el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />

<arg name="scan_topic" default="scan" />

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="odom"/>
<param name="map_update_interval" value="3.0"/>
<param name="maxUrange" value="16.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>

<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot_fer)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>

</syntaxhighlight>

*'''Nota''': los archivos de configuración del ''package'' "move_base" se han obtenido del ''package'' "turtlebot_navigation", carpeta "config", y se han situado en nuestro ''package'' en una carpeta llamada, igualmente, "config".
Para ejecutar "crear_mapa.launch", que inicia la Kinect, "slam_gmaping" y "move_base", se debe introducir el siguiente comando en un terminal del PC robot:

<syntaxhighlight>roslaunch turtlebot_fer crear_mapa.launch</syntaxhighlight>

Para el desplazamiento del [http://www.turtlebot.com Turtlebot] a través del espacio a mapear se empleará el teleoperador por teclado, ejecutando en un terminal del PC robot el siguiente comando:

<syntaxhighlight>roslaunch turtlebot_teleop keyboard_teleop.launch</syntaxhighlight>

*'''Nota''': Para la correcta realización del mapa es necesario dar varias pasadas por el espacio a mapear, saliendo de un punto de partida y regresando a él para cerrar el bucle.
Para poder visualizar el proceso de creación del mapa, en un terminal del PC estación de trabajo se ejecutará el siguiente comando, que lanza el entorno de visualización "rviz" contenido en [http://www.ros.org ROS]:

<syntaxhighlight>rosrun rviz rviz -d `rospack find turtlebot_navigation`/nav_rviz.vcg</syntaxhighlight>

El resultado obtenido después de varias pasadas por el entorno a mapear es el siguiente (Laboratorio F6, robótica):

[[file:turtlebot_mapa.png|center]]

En el siguiente vídeo puede verse el el proceso de creación del mapa:
<center><videoflash>nfbD1vOdIU8</videoflash></center>

<center>[http://www.youtube.com/watch?v=nfbD1vOdIU8 Ver vídeo en YouTube]</center>

Se creará una carpeta llamada "mapas" dentro de nuestro ''package'', donde se guardará el mapa con el nombre "labf6" introduciendo el siguiente comando en un terminal del PC robot:

<syntaxhighlight>rosrun map_server map_saver -f /home/turtlebot/ros_workspace/turtlebot_fer/mapas/labf6</syntaxhighlight>

==Navegación por el entorno mapeado==

Con el mapa ya creado, para que el robot se desplace por él se hará uso del ''stack'' proporcionado por ROS "navigation", que contiene los ''packages'' "map_server", "amcl" (adaptive Monte Carlo localization) y “move_base”. Cada uno de ellos, respectivamente, carga el mapa creado, ejecuta el método probabilístico de localización adaptativa Monte Carlo y planifica la ruta para ir del origen al destino dentro del mapa evitando los obstaculos.
El ''package'' “move_base” se encarga de la planificación de rutas, navegación global, y evitación de obstáculos, navegación local. En la siguiente imagen se muestra el diagrama de bloques de la estructura de este ''package'':

[[file:turtlebot_move_base.png|center]]

Cuando el robot se encuentra atascado el procedimiento, por defecto, que sigue “move_base” para intentar recuperarlo es el que se muestra en la siguiente imagen, pudiendo ser modificado este comportamiento en el parámetro “recovery_behaviors”, por otro de los disponibles:

[[file:turtlebot_move_base_behabior.png|center|450px]]

Se creará, en la carpeta "launch" de nuestro ''package'', el archivo "usar_mapa.launch" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<include file="$(find turtlebot_bringup)/kinect.launch" />

<arg name="map_file" default="$(find turtlebot_fer)/mapas/labf6.yaml"/>

<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan" />

<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>

<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>

<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>

<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>

<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot_fer)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot_fer)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_fer)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>

</syntaxhighlight>

Para ejecutar "usar_mapa.launch", que inicia la Kinect, "map_server", "amcl" y "move_base", se debe introducir el siguiente comando en un terminal del PC robot:

roslaunch turtlebot_fer usar_mapa.launch map_file:=/home/turtlebot/ros_workspace/turtlebot_fer/mapas/labf6.yaml

Para interactuar con el robot se debe iniciar el entrono de visualización "rviz", ejecutando el siguiente comando en un terminal del PC estación de trabajo:

rosrun rviz rviz -d `rospack find turtlebot_navigation`/nav_rviz.vcg

Primero se deberá indicar la posición y orientación aproximada del robot dentro del mapa, pulsando en el botón "2D Pose Estimate" y posteriormente pulsando sobre el mapa en la posición estimada, sin soltar el botón del ratón indicar la orientación estimada y soltar. El modelo del robot se situará ahora en esta posición, rodeado por una nube de puntos en color rojo como se muestra en la imagen:

[[file:turtlebot_estimate.png|center]]

Para indicar el punto de destino dentro del mapa, pulsando "2D Nav Goal" y posteriormente pulsando sobre el mapa en la posición de destino, sin soltar el botón del ratón se indicara la orientación final. En este momento el robot comenzará a moverse siguiendo la trayectoria calculada, línea de color verde, hasta alcanzar el punto de destino, como se muestra en la imagen:

[[file:turtlebot_goal.png|center]]

En el siguiente vídeo puede verse el el proceso de indicación de la situación estimada del robot y navegación al destino marcado:

<center><videoflash>6CLc72Llfow</videoflash></center>

<center>[http://www.youtube.com/watch?v=6CLc72Llfow Ver vídeo en YouTube]</center>

En el siguiente vídeo se puede ver al [http://www.turtlebot.com Turtlebot] usando la navegación autónoma, y como esquiva un objeto no incluido en el mapa, después de ejecutar el comportamiento de recuperación tras un atascamiento, recalculando la ruta:

<center><videoflash>Sh4zswP1fVo</videoflash></center>

<center>[http://www.youtube.com/watch?v=Sh4zswP1fVo Ver vídeo en YouTube]</center>

*'''Nota''': Para poder probar la navegación autónoma en el [[#Simulador|simulador]] es necesario añadir la siguiente línea de código al archivo "robot.launch", situado en la ruta '''opt/ros/electric/stacks/turtlebot_simulator/turtlebot_gazebo/launch''':

<param name="output_frame" value="odom"/>

Esta línea deberá añadirse en la ejecución del ''node'' "robot_pose_ekf". Quedando el archivo "robot.launch" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />

<node name="spawn_turtlebot_model" pkg="gazebo" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf
-param robot_description -model turtlebot" respawn="false" output="screen"/>

<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
<rosparam command="load" file="$(find turtlebot_bringup)/config/diagnostics.yaml" />
</node>

<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="30.0" />
</node>



<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="false"/>
<param name="vo_used" value="false"/>
<param name="output_frame" value="odom"/> 
</node>


<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
<param name="max_rate" value="20.0"/>
<remap from="cloud_in" to="/camera/depth/points"/>
<remap from="cloud_out" to="cloud_throttled"/>
</node>


<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.15"/>
<param name="max_height" value="0.15"/>
<remap from="cloud" to="/cloud_throttled"/>
</node>


<node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.025"/>
<param name="max_height" value="0.025"/>
<remap from="cloud" to="/cloud_throttled"/>
<remap from="scan" to="/narrow_scan"/>
</node>

</launch>

</syntaxhighlight>

----
{| style="border: solid 0px white; width: 100%"
!Otros artículos
----
|-
| valign="top" width="100%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
|}
----

CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

2014-10-28T19:32:21Z

Fernando:

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Teleoperation of CeRVaNTeS=

We need a teleoperation system in order to take the control of the robot to make specific tasks. Currently we use the [http://es.wikipedia.org/wiki/Secure_Shell ssh] connection between the robot PC and our PC, using the keyboard. This method use a [http://es.wikipedia.org/wiki/Wi-Fi wifi] network that can fail or not available. For this reason the best option is to use a device plugged in to the PC robot with its own network. The well-chosen device is the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 wireless controller of microsoft].

=xbox360 controller=

==Description of buttons and axes==

This controller has 6 axes and 14 buttons that are distributed like is shown below:

* Left pad: 2 axes and 1 button.
* Right pad: 2 axes and 1 button.
* Left trigger: 1 axe.
* Right trigger: 1 axe.
* Cross: 4 buttons.
* Left side buttons: 4 buttons.
* Left button: 1 button.
* Right button: 1 button.
* "Back" button: 1 button.
* "Start" button: 1 button.

The function assigned to the buttons and axes of the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for teleoperation of CeRVaNTeS is shown in the next picture:

[[file:xbox360_control-CeRVaNTeS-EN.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for CeRVaNTeS.]]

==Program==

The program assigns the function to the buttons and axes. The program subscribes to the ''topic'' "joy" where is published the state of the buttons and axes and publishes the ''topics'' "Base_Control" and "Group_Motor_Control" for the control of the base and the [[CeRVaNTeS' arm control (maxon+epos2)|arm]]. We have to execute the next command in a terminal in order to create a new ''package'' named "cervantes_teleop" within our catkin workspace:

<syntaxhighlight>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

We will create a new file named "teleop_xbox360.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

We will add the next code line to the file "CMakeLists.txt" of the created ''package'' where we indicate the name of the executable and the path of the file to compile:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

We will execute the next command in a terminal, placed in the root folder of the our catkin work space, in order to compile and generate the executable:

<syntaxhighlight>roscd cervantes_teleop
make</syntaxhighlight>

We will create a file named "teleop_xbox360.launch" within the folder "launch" of the created ''package'' with the content that is shown below in order to start all the necessary programs:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the teleoperator:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

In the next video we can see de teleoperation of CeRVaNTeS in the [http://gazebosim.org/ gazebo] simulator:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

2014-10-28T19:31:44Z

Fernando:

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Teleoperation of CeRVaNTeS=

We need a teleoperation system in order to take the control of the robot to make specific tasks. Currently we use the [http://es.wikipedia.org/wiki/Secure_Shell ssh] connection between the robot PC and our PC, using the keyboard. This method use a [http://es.wikipedia.org/wiki/Wi-Fi wifi] network that can fail or not available. For this reason the best option is to use a device plugged in to the PC robot with its own network. The well-chosen device is the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 wireless controller of microsoft].

=xbox360 controller=

==Description of buttons and axes==

This controller has 6 axes and 14 buttons that are distributed like is shown below:

* Left pad: 2 axes and 1 button.
* Right pad: 2 axes and 1 button.
* Left trigger: 1 axe.
* Right trigger: 1 axe.
* Cross: 4 buttons.
* Left side buttons: 4 buttons.
* Left button: 1 button.
* Right button: 1 button.
* "Back" button: 1 button.
* "Start" button: 1 button.

The function assigned to the buttons and axes of the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for teleoperation of CeRVaNTeS is shown in the next picture:

[[file:xbox360_control-CeRVaNTeS-EN.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for CeRVaNTeS.]]

==Program==

The program assigns the function to the buttons and axes. The program subscribes to the ''topic'' "joy" where is published the state of the buttons and axes and publishes the ''topics'' "Base_Control" and "Group_Motor_Control" for the control of the base and the [[CeRVaNTeS' arm control (maxon+epos2)|arm]]. We have to execute the next command in a terminal in order to create a new ''package'' named "cervantes_teleop" within our catkin workspace:

<syntaxhighlight>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

We will create a new file named "teleop_xbox360.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

We will add the next code line to the file "CMakeLists.txt" of the created ''package'' where we indicate the name of the executable and the path of the file to compile:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

We will execute the next command in a terminal, placed in the root folder of the our catkin work space, in order to compile and generate the executable:

<syntaxhighlight>roscd cervantes_teleop
make</syntaxhighlight>

We will create a file named "teleop_xbox360.launch" within the folder "launch" of the created ''package'' with the content that is shown below in order to start all the necessary programs:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the teleoperator:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

In the next video we can see de teleoperation of CeRVaNTeS in the [http://gazebosim.org/ gazebo] simulator:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

Mobile manipulation

2014-10-27T23:45:30Z

Fernando:

File:Cervantes portada.png

2014-10-27T23:17:49Z

Fernando: CeRVaNTeS

CeRVaNTeS

File:Myrabot portada.png

2014-10-27T23:16:25Z

Fernando: MYRABot

MYRABot

CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

2014-10-15T10:39:43Z

Fernando: /* Program */

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Teleoperation of CeRVaNTeS=

We need a teleoperation system in order to take the control of the robot to make specific tasks. Currently we use the [http://es.wikipedia.org/wiki/Secure_Shell ssh] connection between the robot PC and our PC, using the keyboard. This method use a [http://es.wikipedia.org/wiki/Wi-Fi wifi] network that can fail or not available. For this reason the best option is to use a device plugged in to the PC robot with its own network. The well-chosen device is the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 wireless controller of microsoft].

=xbox360 controller=

==Description of buttons and axes==

This controller has 6 axes and 14 buttons that are distributed like is shown below:

* Left pad: 2 axes and 1 button.
* Right pad: 2 axes and 1 button.
* Left trigger: 1 axe.
* Right trigger: 1 axe.
* Cross: 4 buttons.
* Left side buttons: 4 buttons.
* Left button: 1 button.
* Right button: 1 button.
* "Back" button: 1 button.
* "Start" button: 1 button.

The function assigned to the buttons and axes of the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for teleoperation of CeRVaNTeS is shown in the next picture:

[[file:xbox360_control-CeRVaNTeS-EN.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for CeRVaNTeS.]]

==Program==

The program assigns the function to the buttons and axes. The program subscribes to the ''topic'' "joy" where is published the state of the buttons and axes and publishes the ''topics'' "Base_Control" and "Group_Motor_Control" for the control of the base and the [[CeRVaNTeS' arm control (maxon+epos2)|arm]]. We have to execute the next command in a terminal in order to create a new ''package'' named "cervantes_teleop" within our catkin workspace:

<syntaxhighlight>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

We will create a new file named "teleop_xbox360.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

We will add the next code line to the file "CMakeLists.txt" of the created ''package'' where we indicate the name of the executable and the path of the file to compile:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

We will execute the next command in a terminal, placed in the root folder of the our catkin work space, in order to compile and generate the executable:

<syntaxhighlight>roscd cervantes_teleop
make</syntaxhighlight>

We will create a file named "teleop_xbox360.launch" within the folder "launch" of the created ''package'' with the content that is shown below in order to start all the necessary programs:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the teleoperator:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

In the next video we can see de teleoperation of CeRVaNTeS in the [http://gazebosim.org/ gazebo] simulator:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm and base control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

CeRVaNTeS' arm and base control (maxon+epos2)

2014-10-02T15:44:40Z

Fernando: /* Base */

CeRVaNTeS' arm and base control (maxon+epos2)

2014-10-02T15:43:58Z

Fernando: /* Maxon motors arm and base */

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-10-02T15:41:15Z

Fernando: /* Brazo */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

Los accionamientos del brazo y la base están compuestos por los siguientes elementos:

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder incremental 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

'''Nota''': Para conocer la posición absoluta de las articulaciones del brazo se dispone de un potenciómetro conectado a la salida de la reductora, en cada una.

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder incrementales 2048ppv

'''Nota''': Las controladoras Epos2 se conectan en serie a través del puerto CANopen, cada una tiene una ID diferente. El PC actúa de maestro y se conecta al puerto USB de la primera controladora del conjunto.

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-10-02T15:38:59Z

Fernando: /* Base */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

Los accionamientos del brazo y la base están compuestos por los siguientes elementos:

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder incremental 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

'''Nota''': Para conocer la posición absoluta de las articulaciones del brazo se dispone de un potenciómetro en cada una.

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder incrementales 2048ppv

'''Nota''': Las controladoras Epos2 se conectan en serie a través del puerto CANopen, cada una tiene una ID diferente. El PC actúa de maestro y se conecta al puerto USB de la primera controladora del conjunto.

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

CeRVaNTeS' arm and base control (maxon+epos2)

2014-10-01T09:27:55Z

Fernando: /* Maxon motors arm and base */

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T20:13:11Z

Fernando: /* Brazo y base motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

Los accionamientos del brazo y la base están compuestos por los siguientes elementos:

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder incrementales 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder incremental 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

'''Nota''': Para conocer la posición absoluta de las articulaciones del brazo se dispone de un potenciómetro en cada una.

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder incrementales 2048ppv

'''Nota''': Las controladoras Epos2 se conectan en serie a través del puerto CANopen, cada una tiene una ID diferente. El PC actúa de maestro y se conectan al puerto USB de la primera controladora del conjunto.

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T20:05:29Z

Fernando: /* Brazo y base motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

Los accionamientos del brazo y la base están compuestos por los siguientes elementos:

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder relativos 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder relativos 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder relativo 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

'''Nota''': Para conocer la posición absoluta de las articulaciones del brazo se dispone de un potenciómetro en cada una.

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder relativos 2048ppv

'''Nota''': Las controladoras Epos2 se conectan en serie a través del puerto CANopen, cada una tiene una ID diferente. El PC actúa de maestro y se conectan al puerto USB de la primera controladora del conjunto.

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T19:52:52Z

Fernando: /* Brazo y base motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

Los accionamientos del brazo y la base están compuestos por los siguientes elementos:

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder relativos 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder relativos 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder relativo 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder relativos 2048ppv

'''Nota''': Para conocer la posición absoluta de las articulaciones del brazo se dispone de un potenciómetro en cada una.

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T19:46:19Z

Fernando: /* Brazo y base motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder absolutos 1024ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder absolutos 1024ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder absoluto 1024ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

==Base==

* Base (rueda izquierda/derecha) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 86:1 + 2 encoder absolutos 2048ppv

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T19:40:47Z

Fernando: /* Brazo y base motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

==Brazo==

* Hombro (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE 30 (60W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder absolutos 256ppv
* Codo (giro/inclinación) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] RE-max 29 (22W) + 2 controladoras Epos2 + 2 reductoras planetarias 246:1 + 2 encoder absolutos 256ppv
* Muñeca (giro) = 1 motor [http://www.maxonmotor.es/maxon/view/content/index Maxon] A-max 22 (6W) + 1 controladora Epos2 + 1 reductora planetaria 270:1 + 1 encoder absoluto 256ppv
* Muñeca (inclinación/ladeo) = 2 motores [http://www.maxonmotor.es/maxon/view/content/index Maxon] EC 20 flat (5W) + + 2 controladoras Epos2 + 2 reductoras planetarias 270:1 + sensor [http://es.wikipedia.org/wiki/Sensor_de_efecto_Hall Hall] 16ppv

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

Control brazo y base CeRVaNTeS (maxon+epos2)

2014-09-30T17:55:30Z

Fernando: /* Brazo y base Motores Maxon */

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo y base motores Maxon=

=ROS y Epos2=

Para la realización del driver de comunicación entre [http://wiki.ros.org/ ROS] y las controladoras Epos2 de [http://www.maxonmotor.es/maxon/view/content/index Maxon] nos hemos basado en el proyecto [https://code.google.com/p/wpi-rover/ wpi-rover], el cual consiste en un ''wrapper'' o ''package'' de [http://wiki.ros.org/ ROS] que utiliza las librerias para [http://www.linux.com/ linux] de Epos2.

[[Mobile manipulation | < volver a principal]]

MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

2014-09-29T11:12:21Z

Fernando:

Voice control (sphinx+festival)

2014-09-29T11:11:54Z

Fernando:

Integration of MYRAbot in moveIt! (gazebo+moveIt!)

2014-09-29T11:11:17Z

Fernando:

MYRAbot model for simulation (urdf+gazebo)

2014-09-29T11:10:18Z

Fernando:

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

| valign="top" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]] 
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Components of MYRAbot model=

We have started creating a model of the MYRAbot's structure and we have added the mobile base [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba], the [http://www.xbox.com/es-ES/Kinect kinect] camera, the [[MYRAbot's arm model for simulation (urdf+gazebo)|arm model]] and the webcam [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000].

First, we will create a new ''package'' with the necessary dependences for our programs (urdf std_msgs sensor_msgs tf roscpp). We will execute the next commands in a terminal:

<syntaxhighlight>
cd ~/ros_workspace
roscreate-pkg myrabot_fer_modelo urdf std_msgs sensor_msgs tf roscpp</syntaxhighlight>

==URDF MYRAbot's structure==

We have made the 3D model of the structure with a [http://en.wikipedia.org/wiki/Computer-aided_design CAD] software from which we use the parts with a complex geometry.

[[file:estructura_MYRAbot_modelo.jpg|thumb|500px|center|3D [http://en.wikipedia.org/wiki/Computer-aided_design CAD] design of MYRAbot's structure]]

We have divided the model in two files, main file and macros file. For the main file, we will create a file named "estructura-myrabot.xacro" within the folder "urdf" of the created package with the content that is shown below:

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot>

<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot-macros.xacro" />

<macro name="estructura_myrabot" params="parent *origin">

<joint name="world_joint" type="fixed">
<parent link="${parent}"/>
<child link="e_base_1_link"/>
<insert_block name="origin" />
</joint>

<link name="e_base_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
</collision>
</link>

<separador_base id="1" />
<e_separador_base id="1" xsim="1" ysim="1" />

<separador_base id="2" />
<e_separador_base id="2" xsim="1" ysim="-1" />

<separador_base id="3" />
<e_separador_base id="3" xsim="-1" ysim="-1" />

<separador_base id="4" />
<e_separador_base id="4" xsim="-1" ysim="1" />

<joint name="base_2" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_base_2_link"/>
<origin xyz="0 0 0.135" rpy="0 0 0" />
</joint>

<link name="e_base_2_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
</collision>
</link>

<joint name="pilar" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_pilar_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_pilar_link">
<inertial>
<mass value="0.08"/>
<origin rpy="0 0 0" xyz="0 0 ${0.7/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="ultrasonidos" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_ultrasonidos_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_ultrasonidos_link">
<inertial>
<mass value="0.04"/>
<origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="botonera" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_botonera_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_botonera_link">
<inertial>
<mass value="0.06"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
</collision>
</link>

<joint name="start_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_start_button_1_link"/>
<origin xyz="-0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_start_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
</collision>
</link>

<joint name="start_button_2" type="fixed">
<parent link="e_start_button_1_link"/>
<child link="e_start_button_2_link"/>
<origin xyz="0.0 0 0.002" rpy="0 0 0" />
</joint>

<link name="e_start_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
</collision>
</link>

<joint name="emergency_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_emergency_button_1_link"/>
<origin xyz="0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
</collision>
</link>

<joint name="emergency_button_2" type="fixed">
<parent link="e_emergency_button_1_link"/>
<child link="e_emergency_button_2_link"/>
<origin xyz="0.0 0 0.015" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
</collision>
</link>

<joint name="base_kinect" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_kinect_link"/>
<origin xyz="0 0 ${0.395-0.155}" rpy="0 0 0" />
</joint>

<link name="e_base_kinect_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="base_brazo_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_brazo_1_link"/>
<origin xyz="0 0 ${0.708-(0.395-0.155)}" rpy="0 0 0" />
</joint>

<link name="e_base_brazo_1_link">
<inertial>
<mass value="0.03"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<separador_base_brazo id="1" />
<e_separador_base_brazo id="1" xsim="1" ysim="1" />

<separador_base_brazo id="2" />
<e_separador_base_brazo id="2" xsim="1" ysim="-1" />

<separador_base_brazo id="3" />
<e_separador_base_brazo id="3" xsim="-1" ysim="-1" />

<separador_base_brazo id="4" />
<e_separador_base_brazo id="4" xsim="-1" ysim="1" />

<joint name="base_brazo_2" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_base_brazo_2_link"/>
<origin xyz="0 0 0.074" rpy="0 0 0" />
</joint>

<link name="e_base_brazo_2_link">
<inertial>
<mass value="0.005"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="soporte_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_soporte_1_link"/>
<origin xyz="0 0 ${0.995-(0.708-0.155)-(0.395-0.155)}" rpy="0 0 0" />
</joint>

<link name="e_soporte_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0 0 ${0.64/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="soporte_2" type="fixed">
<parent link="e_soporte_1_link"/>
<child link="e_soporte_2_link"/>
<origin xyz="0 0.2 0.61" rpy="-0.12 0 0" />
</joint>

<link name="e_soporte_2_link">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 ${0.06/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="monitor" type="fixed">
<parent link="e_soporte_2_link"/>
<child link="e_monitor_link"/>
<origin xyz="0 -0.2 -0.09" rpy="0 0 0" />
</joint>

<link name="e_monitor_link">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0 0 ${0.180/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="pantalla" type="fixed">
<parent link="e_monitor_link"/>
<child link="e_pantalla_link"/>
<origin xyz="0.135 ${0.135-0.005-0.03-0.0001} 0.086" rpy="0 0 0" />
</joint>

<link name="e_pantalla_link">
<inertial>
<mass value="0.0001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.130" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.127" />
</geometry>
</collision>
</link>

<gazebo_propiedades nombre="e_base_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_separador_base_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_4_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_4_link" material="Grey" />

<gazebo_propiedades nombre="e_pilar_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_kinect_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_botonera_link" material="LightWood" />

<gazebo_propiedades nombre="e_start_button_1_link" material="Black" />

<gazebo_propiedades nombre="e_start_button_2_link" material="Green" />

<gazebo_propiedades nombre="e_emergency_button_1_link" material="Grey" />

<gazebo_propiedades nombre="e_emergency_button_2_link" material="Red" />

<gazebo_propiedades nombre="e_soporte_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_soporte_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_monitor_link" material="LightWood" />

<gazebo_propiedades nombre="e_pantalla_link" material="Black" />

</macro>

</robot>

</syntaxhighlight>

We will add the next files within the folder "meshes" of our ''package'':

* [http://www.fernando.casadogarcia.es/files/e_pilar.stl e_pilar.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_kinect.stl e_base_kinect.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_brazo_1.stl e_base_brazo_1.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_brazo_2.stl e_base_brazo_2.stl]
* [http://www.fernando.casadogarcia.es/files/e_soporte_1.stl e_soporte_1.stl]
* [http://www.fernando.casadogarcia.es/files/e_soporte_2.stl e_soporte_2.stl]
* [http://www.fernando.casadogarcia.es/files/e_monitor.stl e_monitor.stl]
* [http://www.fernando.casadogarcia.es/files/e_ultrasonidos.stl e_ultrasonidos.stl]

For the macros file, we will create a file named "estructura-myrabot-macros.xacro" within the folder "urdf" of our package with the content that is shown below:

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot>

<macro name="default_inertia_e">
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>

<macro name="separador_base" params="id">
<joint name="separador_base_${id}" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_separador_base_${id}_link"/>
<origin xyz="0.135 0.135 0.015" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base" params="id xsim ysim">
<link name="e_separador_base_${id}_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
<material name="metal">
<color rgba="${211/255} ${211/255} ${211/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
</collision>
</link>
</macro>

<macro name="separador_base_brazo" params="id">
<joint name="separador_base_brazo_${id}" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_separador_base_brazo_${id}_link"/>
<origin xyz="0.135 ${0.135-0.082} 0.075" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base_brazo" params="id xsim ysim">
<link name="e_separador_base_brazo_${id}_link">
<inertial>
<mass value="0.002"/>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
</collision>
</link>
</macro>

<macro name="gazebo_propiedades" params="nombre material">
<gazebo reference="${nombre}">
<material>Gazebo/${material}</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</macro>

</robot>

</syntaxhighlight>

==URDF Roomba==

We have used the model of the mobile base [http://store.irobot.com/family/index.jsp?categoryId=2591511&s=A-ProductAge create] of the robot [http://www.turtlebot.com/ turtlebot], because the vacuum [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba] has similar sensors and physics. We have to install the ''stacks'' [http://wiki.ros.org/turtlebot turtlebot] and [http://wiki.ros.org/turtlebot_simulator turtlebot_simulator] that contain the robot model and the ''plugins'' for [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. We have divided the model in two files, main file and ''plugins'' file. For the main file, we will create a file named "roomba.xacro" within the folder "urdf" of the created package with the content that is shown below:

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

<include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>

<xacro:macro name="roomba">

<material name="Green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>

<link name="base_footprint">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="Green" />
</visual>

<collision>
<origin xyz="0 0 0.017" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>

<link name="base_link">
<inertial>
<mass value="3.6" />
<origin xyz="0 0 ${0.063/2}" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
</inertial>

<visual>
<origin xyz=" 0 0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1" />
</material>
</visual>

<collision>
<origin xyz="0.0 0.0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
</collision>
</link>

<link name="wall_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="left_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="right_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="leftfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>

<link name="rightfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0.017" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>

<joint name="base_wall_sensor_joint" type="fixed">
<origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />
<parent link="base_link"/>
<child link="wall_sensor_link" />
</joint>

<joint name="base_left_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="left_cliff_sensor_link" />
</joint>

<joint name="base_right_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="right_cliff_sensor_link" />
</joint>

<joint name="base_leftfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="leftfront_cliff_sensor_link" />
</joint>

<joint name="base_rightfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="rightfront_cliff_sensor_link" />
</joint>

<link name="left_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>

<joint name="left_wheel_joint" type="continuous">
<origin xyz="0 0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="right_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>

<joint name="right_wheel_joint" type="continuous">
<origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="rear_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.015" />
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.015" />
</geometry>
</collision>
</link>

<joint name="rear_castor_joint" type="fixed">
<origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="rear_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="front_wheel_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.018" />
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.018" />
</geometry>
</collision>
</link>

<joint name="front_castor_joint" type="fixed">
<origin xyz="0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="front_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<joint name="gyro_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0 0 0.04" rpy="0 0 0" />
<parent link="base_link"/>
<child link="gyro_link"/>
</joint>
<link name="gyro_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
</inertial>
</link>

<joint name="laser_joint" type="fixed">
<origin xyz="-0.065 0 0.57" rpy="0 0 0" />
<parent link="base_link" />
<child link="laser" />
</joint>

<link name="laser" />

<gaezebo>
<material>Gazebo/White</material>
</gaezebo>

<myrabot_sim_imu/>
<myrabot_sim_laser/>
<myrabot_sim_roomba/>
<myrabot_sim_wall_sensors/>

</xacro:macro>

</robot>

</syntaxhighlight>

For the ''plugins'' file, we will create a file named "roomba-gazebo.xacro" within the folder "urdf" of our package with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="myrabot_sim_imu">
<gazebo>
<controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
<robotNamespace>/</robotNamespace>
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<bodyName>gyro_link</bodyName>
<topicName>imu/data</topicName>
<gaussianNoise>${0.0017*0.0017}</gaussianNoise>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="imu_position"/>
</controller:gazebo_ros_imu>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_laser">
<gazebo reference="laser">
<sensor:ray name="laser">
<rayCount>180</rayCount>
<rangeCount>180</rangeCount>
<laserCount>1</laserCount>

<origin>0.0 0.0 0.0</origin>
<displayRays>false</displayRays>

<minAngle>-${60/2}</minAngle>
<maxAngle>${60/2}</maxAngle>

<minRange>0.08</minRange>
<maxRange>5.0</maxRange>
<resRange>0.01</resRange>
<updateRate>20</updateRate>
<controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>scan</topicName>
<frameName>laser</frameName>
<interface:laser name="gazebo_ros_laser_iface" />
</controller:gazebo_ros_laser>
</sensor:ray>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_roomba">
<gazebo>
<controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_roomba.so">
<alwaysOn>true</alwaysOn>
<node_namespace>turtlebot_node</node_namespace>
<left_wheel_joint>left_wheel_joint</left_wheel_joint>
<right_wheel_joint>right_wheel_joint</right_wheel_joint>
<front_castor_joint>front_castor_joint</front_castor_joint>
<rear_castor_joint>rear_castor_joint</rear_castor_joint>
<wheel_separation>.260</wheel_separation>
<wheel_diameter>0.066</wheel_diameter>
<base_geom>base_link_geom_base_link</base_geom>
<updateRate>40</updateRate>
<torque>1.0</torque>
</controller:gazebo_ros_roomba>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_wall_sensors">
<gazebo reference="wall_sensor_link">
<sensor:ray name="wall_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.0160</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="left_cliff_sensor_link">
<sensor:ray name="left_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="right_cliff_sensor_link">
<sensor:ray name="right_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="leftfront_cliff_sensor_link">
<sensor:ray name="leftfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="rightfront_cliff_sensor_link">
<sensor:ray name="rightfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>fan</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="left_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>

<gazebo reference="right_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>
<gazebo reference="rear_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>

<gazebo reference="front_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>
</xacro:macro>
</robot>

</syntaxhighlight>

==URDF kinect==

We will create a file named "kinect.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<property name="M_PI" value="3.14159"/>

<xacro:macro name="kinect" params="parent *origin">

<joint name="base_camera_joint" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="camera_link" />
</joint>

<link name="camera_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>

<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</visual>

<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</collision>

</link>

<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0.018 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_depth_frame" />
</joint>

<link name="camera_depth_frame">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>

<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>

<link name="camera_depth_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<joint name="camera_rgb_joint" type="fixed">
<origin xyz="0 -0.005 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_rgb_frame" />
</joint>

<link name="camera_rgb_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<joint name="camera_rgb_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_rgb_frame" />
<child link="camera_rgb_optical_frame" />
</joint>

<link name="camera_rgb_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<gazebo reference="camera_link">
<sensor:camera name="camera">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>60</hfov>
<nearClip>0.05</nearClip>
<farClip>5</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/camera/image_raw</imageTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/camera/camera_info</cameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>

</xacro:macro>

</robot>

</syntaxhighlight>

==URDF webcam==

We will create a file named "webcam.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="webcam" params="parent *origin">

<joint name="webcam_1" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="e_webcam_1_link" />
</joint>

<link name="e_webcam_1_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="webcam_2" type="fixed">
<origin xyz="0 ${0.135-0.04-(0.02-0.009)} ${(0.185/2)+0.0045}" rpy="0.82 0 0" />
<parent link="e_webcam_1_link" />
<child link="e_webcam_2_link" />
</joint>

<link name="e_webcam_2_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="webcam_optica" type="fixed">
<origin xyz="0.135 -0.0285 0" rpy="0 0 -1.57" />
<parent link="e_webcam_2_link" />
<child link="e_webcam_optica_link" />
</joint>

<link name="e_webcam_optica_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
</collision>
</link>

<gazebo reference="e_webcam_1_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_2_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_optica_link">
<material>myrabot_fer/White</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_optica_link">
<sensor:camera name="webcam">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>63.09</hfov>
<nearClip>0.02</nearClip>
<farClip>3</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="webcam_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/webcam/image_raw</imageTopicName>
<pointCloudTopicName>/webcam/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/webcam/camera_info</cameraInfoTopicName>
<frameName>e_webcam_optica_link</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>

</xacro:macro>

</robot>

</syntaxhighlight>

We will add the next files within the folder "meshes" of our ''package'':

* [http://www.fernando.casadogarcia.es/files/webcam_1.stl webcam_1.stl]
* [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]

==URDF ultrasound sensor==

===Creation of a gazebo plugin===

we will create a new ''package'' with the necessary dependences for our plugins (gazebo sensor_msgs roscpp). We will execute the next commands in a terminal:

<syntaxhighlight>
cd ~/ros_workspace
roscreate-pkg myrabot_gazebo_plugins gazebo sensor_msgs roscpp</syntaxhighlight>

We will create a file named "gazebo_ros_sonar.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang="cpp" enclose="div">

/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <gazebo/Sensor.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <limits>
#include <myrabot_gazebo_plugin/gazebo_ros_sonar.h>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)

GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
{
sensor_ = dynamic_cast<RaySensor*>(parent);
if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");

Param::Begin(&parameters);
topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
frame_id_param_ = new ParamT<std::string>("frameId", "", false);
radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
fov_param_ = new ParamT<double>("fov", 0.05, false);
gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
Param::End();
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosSonar::~GazeboRosSonar()
{
delete topic_param_;
delete frame_id_param_;
delete radiation_param_;
delete fov_param_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::LoadChild(XMLConfigNode *node)
{
ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
topic_param_->Load(node);
frame_id_param_->Load(node);
radiation_param_->Load(node);
fov_param_->Load(node);
gaussian_noise_->Load(node);
}

///////////////////////////////////////////////////////
// Initialize the controller
void GazeboRosSonar::InitChild()
{
Range.header.frame_id = **frame_id_param_;

if (**radiation_param_==std::string("ultrasound"))
Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
else
Range.radiation_type = sensor_msgs::Range::INFRARED;

Range.field_of_view = **fov_param_;
Range.max_range = sensor_->GetMaxRange();
Range.min_range = sensor_->GetMinRange();

sensor_->SetActive(false);
node_handle_ = new ros::NodeHandle("");
publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
}

////////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double GazeboRosSonar::GaussianKernel(double mu,double sigma)
{
// using Box-Muller transform to generate two independent standard normally disbributed normal variables
// see wikipedia
double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
//double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
// we'll just use X
// scale to our mu and sigma
X = sigma * X + mu;
return X;
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosSonar::UpdateChild()
{
if (!sensor_->IsActive()) sensor_->SetActive(true);

Range.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();

for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
double ray = sensor_->GetRange(i);
if (ray < Range.range) Range.range = ray;
}

Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
publisher_.publish(Range);
}

////////////////////////////////////////////////////////////////////////////////
// Finalize the controller
void GazeboRosSonar::FiniChild()
{
sensor_->SetActive(false);
node_handle_->shutdown();
delete node_handle_;
}

</syntaxhighlight>

We will create a file named "gazebo_ros_sonar.h" within the path "include/myrabot_gazebo_plugins/" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang="cpp" enclose="div">

/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>

#include <gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/sensors/RaySensor.hh>

#include <ros/callback_queue.h>
#include <sensor_msgs/Range.h>

namespace gazebo
{

class GazeboRosSonar : public gazebo::SensorPlugin
{
public:
GazeboRosSonar();
virtual ~GazeboRosSonar();

protected:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
virtual void Update();

/// \brief Gaussian noise generator
private:
double GaussianKernel(double mu,double sigma);

private:
gazebo::sensors::RaySensorPtr sensor_;

ros::NodeHandle* node_handle_;
ros::Publisher publisher_;

sensor_msgs::Range range_;

std::string namespace_;
std::string topic_name_;
std::string frame_id_;
std::string radiation_;
double fov_;
double gaussian_noise_;

gazebo::physics::WorldPtr parent_;

common::Time last_time;

gazebo::event::ConnectionPtr updateConnection;

};

} // namespace gazebo

</syntaxhighlight>

We have to add the next lines at the end within the file CMakeLists.txt of the plugins ''package'':

<syntaxhighlight>rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)</syntaxhighlight>

Also, we have to add the next lines before the tag </package> within the file "manifest.xml" of the plugins ''package'':

<syntaxhighlight lang="xml">

<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
<gazebo plugin_path="${prefix}/lib" />
</export>

</syntaxhighlight>

We will execute the next commands in a terminal in order to compile the plugin:

<syntaxhighlight>

roscd myrabot_gazebo_plugins
make

</syntaxhighlight>

===URDF===

We will create a file named "ultrasonidos.xacro" within the folder "urdf" of our robot model ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">

<joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
<parent link="${parent}"/>
<child link="ultrasonidos_${id}_link_fisico"/>
<insert_block name="origin" />
</joint>

<link name="ultrasonidos_${id}_link_fisico">
<inertial>
<mass value="0.001"/>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
</collision>
</link>

<joint name="ultrasonidos_${id}_joint" type="fixed">
<parent link="ultrasonidos_${id}_link_fisico"/>
<child link="ultrasonidos_${id}_link"/>
<origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/>
</joint>

<link name="ultrasonidos_${id}_link">

</link>

<gazebo reference="ultrasonidos_${id}_link_fisico">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="ultrasonidos_${id}_link">
<sensor:ray name="ultrasonidos_${id}">

<displayRays>false</displayRays>

<rayCount>5</rayCount>
<rangeCount>5</rangeCount>
<verticalRayCount>1</verticalRayCount>
<verticalRangeCount>1</verticalRangeCount>

<minAngle>-16</minAngle>
<verticalMinAngle>-16</verticalMinAngle>
<maxAngle>16</maxAngle>
<verticalMaxAngle>16</verticalMaxAngle>

<minRange>0</minRange>
<maxRange>7</maxRange>
<resRange>0.01</resRange>

<updateRate>20</updateRate>

<controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>ultrasonidos_${id}</topicName>
<frameId>ultrasonidos_${id}_link</frameId>
<radiation>ultrasound</radiation>
<fov>32</fov>

</controller:gazebo_ros_sonar>
</sensor:ray>
</gazebo>
</xacro:macro>

</robot>

</syntaxhighlight>

=Assembled of MYRAbot model=

We have to join all the parts that form the MYRAbot. We have to set the parent link and origin of each part. We will create a file named "myrabot.urdf.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="MYRAbot"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">

<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />

<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />

<roomba />

<estructura_myrabot parent="base_link">
<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
</estructura_myrabot>

<kinect parent="e_base_kinect_link">
<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
</kinect>

<brazo parent="e_base_brazo_1_link">
<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
</brazo>

<webcam parent="e_monitor_link">
<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
</webcam>

<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 1.57" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.93" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 4.71" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

</robot>

</syntaxhighlight>

==Display of the model in rviz==

We will create a file named "myrabot_rviz.launch" within the folder "launch" of our ''package'' with the content that is shown below, in order to test in [http://wiki.ros.org/rviz rviz] the correct assembly of the model and the correct performance of the joints.

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

<param name="use_gui" value="true"/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

<node name="rviz" pkg="rviz" type="rviz" />

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the ''launcher'':

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>

The ''launcher'' starts [http://wiki.ros.org/rviz rviz] and the [http://en.wikipedia.org/wiki/Graphical_user_interface GUI] of [http://wiki.ros.org/joint_state_publisher joint_state_publisher]. We have to set in "Global Options" the field "Fixed Frame" to "/base_footprint" ''link''. We can change the position of the joints using the sliders of the [http://en.wikipedia.org/wiki/Graphical_user_interface GUI] of [http://wiki.ros.org/joint_state_publisher joint_state_publisher]. We must obtain something similar to the following:

[[file:modelo_MYRAbot_rviz.png|thumb|500px|center|MYRAbot model in [http://wiki.ros.org/rviz rviz] and [http://wiki.ros.org/joint_state_publisher joint_state_publisher] interface]]

=Load of the model in gazebo=

==Using an only''topic'' for joint states==

We will create a file named "remap_joint_states.cpp" within the folder "src" of our ''package'' with the content that is shown below, in order to publish the joint states of the arm (brazo/joint_states) and the mobile base (joint_states) in a only ''topic'' (joint_states_myrabot):

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "ros/time.h"
#include "sensor_msgs/JointState.h"

sensor_msgs::JointState joint_states_todo;
int cont_brazo = 0;
int cont_roomba = 0;
std::string name[9];
float position[9];
float velocity[9];
float effort[9];

void brazo(const sensor_msgs::JointState& brazo_states)
{
int tam = brazo_states.name.size();

if (::cont_brazo == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i] = brazo_states.name[i];
::position[i] = brazo_states.position[i];
::velocity[i] = brazo_states.velocity[i];
::effort[i] = brazo_states.effort[i];
}

::cont_brazo = 1;
}
}

void roomba(const sensor_msgs::JointState& roomba_states)
{
int tam = roomba_states.name.size();

if (::cont_roomba == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i+5] = roomba_states.name[i];
::position[i+5] = roomba_states.position[i];
::velocity[i+5] = roomba_states.velocity[i];
::effort[i+5] = roomba_states.effort[i];
}

::cont_roomba = 1;
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "remap_joint_states");

ros::NodeHandle n;

ros::Subscriber joint_states_brazo_sub_= n.subscribe("brazo/joint_states", 1, brazo);
ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states", 1, roomba);

ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states_myrabot", 1);

ros::Rate loop_rate(100);

ros::Time ahora;

::joint_states_todo.name = std::vector<std::string>(9);
::joint_states_todo.position = std::vector<double>(9);
::joint_states_todo.velocity = std::vector<double>(9);
::joint_states_todo.effort = std::vector<double>(9);

while (ros::ok())
{

if (::cont_brazo == 1 && ::cont_roomba == 1)
{
for (int i = 0; i < 9; i++)
{
::joint_states_todo.name[i] = ::name[i];
::joint_states_todo.position[i] = ::position[i];
::joint_states_todo.velocity[i] = ::velocity[i];
::joint_states_todo.effort[i] = ::effort[i];
}

ahora = ros::Time::now();

::joint_states_todo.header.stamp = ahora;

joint_states_pub_.publish(::joint_states_todo);

::cont_brazo = 0;
::cont_roomba = 0;
}

ros::spinOnce();

loop_rate.sleep();
}

ros::spin();

return 0;
}

</syntaxhighlight>

==Load in gazebo==

We will create a file named "myrabot_gazebo.launch" within the folder "launch" of our ''package'' with the content that is shown below in order to start [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] and to load the model and the controllers:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
<node name="gazebo_gui" pkg="gazebo" type="gui" />

<group ns="brazo">

<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />

<node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />

<rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>

<node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />

</group>

<node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />

<node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="false"/>
<param name="vo_used" value="false"/>
<param name="output_frame" value="odom"/>
</node>

<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
<param name="max_rate" value="20.0"/>
<remap from="cloud_in" to="/camera/depth/points"/>
<remap from="cloud_out" to="cloud_throttled"/>
</node>

<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>
<param name="min_height" value="-0.15"/>
<param name="max_height" value="0.15"/>
<remap from="cloud" to="/cloud_throttled"/>
</node>

<node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>
<param name="min_height" value="-0.025"/>
<param name="max_height" value="0.025"/>
<remap from="cloud" to="/cloud_throttled"/>
<remap from="scan" to="/narrow_scan"/>
</node>

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the ''launcher'':

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>

We can see the MYRAbot model in vertical position placed in the point xyz=(0 0 0) of a empty world in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo].

[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|MYRAbot model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

==Test program (around the bullring)==

We have made a little program to test the performance of the MYRAbot model. When we push the start button the model revolves in circle and the arm rotates from side to side in vertical position. First, we will create a new ''package'' with the necessary dependences for our programs ([[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp). We will execute the next commands in a terminal:

<syntaxhighlight>
cd ~/ros_workspace
roscreate-pkg myrabot_fer brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp</syntaxhighlight>

We will create a file named "vuelta_al_ruedo.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang="cpp" anclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"

int cont = 0;
int cont1= 0;
int encendido = 0;
brazo_fer::Servos move, on, vel;

void home()
{
if (cont1 == 0)
{
ros::NodeHandle n;

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

brazo_fer::WriteServos home;

::move.base = 511;
::move.arti1 = 511;
::move.arti2 = 511;
::move.arti3 = 511;
::move.pinza = 511;

::on.base = 1;
::on.arti1 = 1;
::on.arti2 = 1;
::on.arti3 = 1;
::on.pinza = 1;

::vel.base = 400;
::vel.arti1 = 400;
::vel.arti2 = 400;
::vel.arti3 = 400;
::vel.pinza = 400;

home.posicion = ::move;
home.par = ::on;
home.velocidad = ::vel;

move_arm_pub_.publish(home);
hand_pub_.publish(home);

::cont1 = 1;

}
}

void inicio(const std_msgs::Bool& inicio)
{
if (inicio.data == true && ::cont == 0)
{
::encendido = 1;
::cont = 1;
}
else if (inicio.data == true && ::cont == 1)
{
::encendido = 0;
::cont = 0;
}
}

void posicion(const brazo_fer::ReadServos& posicion)
{
ros::NodeHandle n;

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

brazo_fer::Servos p = posicion.posicion;
brazo_fer::WriteServos mover_brazo;

home();

if (::encendido == 1)
{
geometry_msgs::Twist mover_base;

mover_base.linear.x = 0.1;
mover_base.angular.z = 0.3;

move_base_pub_.publish(mover_base);

if (::cont1 == 1)
{
::move.base = 718;
::cont1 = 2;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
{
if (::cont1 == 2)
{
::move.base = 304;
::cont1 = 3;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else
{
::cont1 = 1;
}
}
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "vuelta_al_ruedo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio);

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our ''package'' in order to compile and create the executable file:

<syntaxhighlight>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>
roscd myrabot_fer
make
</syntaxhighlight>

We will execute the next command in a terminal in order to launch [http://wiki.ros.org/simulator_gazebo gazebo] with the robot model:

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>

We will execute the next command in a new terminal in order to launch the test program:

<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>

We start the program publishing "true" value in the ''topic'' "start_button". We will execute the next command in a new terminal in order to simulate the start button of the real robot:

<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</syntaxhighlight>

The robot model will start to revolve in circle and the arm to rotate from side to side in vertical position. We can see this in the next video:

<center><videoflash>IY63zJgQYGI</videoflash></center>

<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Watch in YouTube]</center>

=Execution of the program (between the beer and the cola, the platform) choose=

We need to create the objects' models (cans and table) in order to execute the program [[Objects recognition and position calculation (webcam)#To point objects (Choose)|Choose]] in the [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] simulator using the MYRAbot model.

==Creation of the objects' models==

First, we will create a new ''package'' with the necessary dependences for our programs (geometry_msgs gazebo urdf roscpp). We will execute the next commands in a terminal:

<syntaxhighlight>
cd ~/ros_workspace
roscreate-pkg objetos_fer_modelos geometry_msgs gazebo urdf roscpp</syntaxhighlight>

===Table===

We have crated a table with the dimensions 200x75x74 cm (length x width x height). We will create a file named "mesa.xacro" within the folder "urdf" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="mesa" type="floating">
<parent link="world"/>
<child link="mesa_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="mesa_link">
<visual>
<geometry>
<box size="2 0.75 0.03" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<geometry>
<box size="2 0.75 0.03" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<mass value="5" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>

<xacro:macro name="pata" params="id refx refy">
<joint name="pata_${id}" type="fixed">
<parent link="mesa_link"/>
<child link="pata_${id}_link"/>
<origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
</joint>

<link name="pata_${id}_link">
<visual>
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<mass value="3" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
</xacro:macro>

<xacro:pata id="1" refx="1" refy="1"/>
<xacro:pata id="2" refx="1" refy="-1"/>
<xacro:pata id="3" refx="-1" refy="-1"/>
<xacro:pata id="4" refx="-1" refy="1"/>

<gazebo reference="mesa_link">
<material>Gazebo/LightWood</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<xacro:macro name="gazebo_pata" params="id">
<gazebo reference="pata_${id}_link">
<material>PR2/Black</material>
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</xacro:macro>

<xacro:gazebo_pata id="1"/>
<xacro:gazebo_pata id="2"/>
<xacro:gazebo_pata id="3"/>
<xacro:gazebo_pata id="4"/>

</robot>

</syntaxhighlight>

[[file:modelo_mesa.png|thumb|200px|center|Table model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

We have used the default materials included in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] for the table, but we need create new materials with texture for the labels of the cans.

===Creation of new materials for gazebo===

First, we have to add the next lines within the file "manifest.xml" of the created ''package'' in order to add the ''package'' path to the [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] path:

<syntaxhighlight lang="xml">

<export>
<gazebo gazebo_media_path="${prefix}" />
</export>

</syntaxhighlight>

[http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] uses the graphics engine [http://www.ogre3d.org/ OGRE]. We will create a file named "myrabot_fer.material" within the objects ''package'' in the relative path "Media/materials/scripts/" with the content that is shown below in order to define a new material with texture:

<syntaxhighlight>

material material_fer/NEW_MATERIAL_NAME
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture OUR_IMAGE_FILE
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

The image file must be in the relative path "Media/materials/textures/" within the objects ''package''.

===Cans===

====Can of beer====

We will create a file named "lata_cerveza.xacro" within the folder "urdf" of the objects ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>

<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<gazebo reference="lata_link">
<material>material_fer/Amstel</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

</robot>

</syntaxhighlight>

We have to add the next lines within the file "myrabot_fer.material" in order to add the new material named "Amstel":

<syntaxhighlight>

material material_fer/Amstel
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture amstel.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

As follows, we can see the image file used for the texture and a screenshot of the can model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

[[file:amstel.png|thumb|200px|center|Texture of the Can of beer]]

[[file:modelo_lata_cerveza.png|thumb|200px|center|Can of beer in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

====Can of cola====

We will create a file named "lata_cola.xacro" within the folder "urdf" of the objects ''package'' with the content that is shown below:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>

<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<gazebo reference="lata_link">
<material>material_fer/CocaCola</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

</robot>

</syntaxhighlight>

We have to add the next lines within the file "myrabot_fer.material" in order to add the new material named "CocaCola":

<syntaxhighlight>

material Gazebo/CocaCola
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture coca_cola.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

As follows, we can see the image file used for the texture and a screenshot of the can model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

[[file:coca_cola.png|thumb|200px|center|Texture of the can of cola]]

[[file:modelo_lata_cola.png|thumb|200px|center|Can of cola in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

* '''Note''': We have to convert the .xacro files into .urdf in order to load the models in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. We will execute the next commands in a terminal:

<syntaxhighlight>
roscd objetos_fer_modelos
cd urdf
rosrun xacro xacro.py OBJECT_NAME.xacro > OBJECT_NAME.urdf
</syntaxhighlight>

==Execution==

We will create a file named "myrabot_latas.launch" within the "myrabot_fer" ''package'' with the content that is shown below in order to load the models of the objects and the robot in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>

<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />

<node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />

<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />

<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

</syntaxhighlight>

We will execute in a terminal the next commad in order to launch [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] with the robot model and the objects:

<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>

We will execute in a new terminal the next command in order to start the program [[Objects recognition and position calculation (webcam)#To point objects (Choose)|Choose]]:

<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>

We will execute the next command in a terminal in order to publish the identifier of the selected object in the ''topic'' "selected_object":

<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>

The arm points the selected object and after a time the arm returns to the home position. We can see this in the next video:

<center><videoflash>lp2FUFlsFV4</videoflash></center>

<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Watch in YouTube]</center>

=Execution of the program tracking objects=

==Program to move objects in gazebo using the keyboard==

We will create a new file named "mover_objetos.cpp" within the folder "src" of the objects ''package'' with yhe content that is shown below, in order to move an object model (the default object model is Lata_amstel) in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] publishing its position in the ''topic'' "gazebo/set_model_state":

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "gazebo/ModelState.h"
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

#define KEYCODE_Xmas 0x43
#define KEYCODE_Xmenos 0x44
#define KEYCODE_Ymas 0x41
#define KEYCODE_Ymenos 0x42
#define KEYCODE_Zmas 0x77
#define KEYCODE_Zmenos 0x73
#define KEYCODE_XgiroMas 0x61
#define KEYCODE_XgiroMenos 0x64
#define KEYCODE_YgiroMas 0x65
#define KEYCODE_YgiroMenos 0x71

struct termios cooked, raw;

int cont = 0;

double retardo = 0.1;

gazebo::ModelState objeto;

void quit(int sig)
{
tcsetattr(0, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}

void callback(const ros::TimerEvent&)
{

ros::NodeHandle n;

ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

std::string modelo_objeto;

ros::NodeHandle nh("~");

nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));

ros::Time anterior;

if (::cont == 0)
{
anterior = ros::Time::now();

::objeto.model_name = modelo_objeto;

::objeto.reference_frame = "world";

::objeto.pose.position.x = 0;

::objeto.pose.position.y = 0.5;

::objeto.pose.position.z = 0.74;

::objeto.pose.orientation.x = 0;

::objeto.pose.orientation.y = 0;

::objeto.pose.orientation.z = 0;

::objeto.pose.orientation.w = 1;

::cont = 1;

object_pub_.publish(::objeto);

}

char c;

// get the console in raw mode
tcgetattr(0, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(0, TCSANOW, &raw);

puts("");
puts("#####################################################");
puts(" CONTROLES OBJETO");
puts("#####################################################");
puts("");
puts("Up arrow:_______________________ Move up object");
puts("Down arrow:_____________________ Move down object");
puts("Left arrow:_____________________ Move left object");
puts("Right arrow:____________________ Move right object");
puts("W key:__________________________ Move forward object");
puts("S key:__________________________ Move backward object");
puts("A key:__________________________ Tilt + x axis object");
puts("D key:__________________________ Tilt - x axis object");
puts("Q key:__________________________ Tilt + y axis object");
puts("E key:__________________________ Tilt - y axis object");

while (ros::ok())
{

// get the next event from the keyboard
if(read(0, &c, 1) < 0)
{
perror("read():");
exit(-1);
}

if (c == KEYCODE_Xmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Xmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;

anterior = ros::Time::now();
}
}

object_pub_.publish(::objeto);

}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "mover_objetos");

ros::NodeHandle n;

ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

signal(SIGINT,quit);

ros::Timer timer = n.createTimer(ros::Duration(1), callback);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line at the end of the file "CMakeLists.txt" of the objects ''package'' in order to compile the program and make the executable:

<syntaxhighlight>rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)</syntaxhighlight>

We will execute the nest commands in a terminal in order to compile the program and make the executable:

<syntaxhighlight>
roscd objetos_fer_modelos
make
</syntaxhighlight>

==Execution==

We will create a file named "myrabot_lata.launch" within the folder "launch" of the "myrabot_fer" ''package'' with the content that is shown below, in order to load the models in the corrects positions:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>

<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />

<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />

<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the launcher:

<syntaxhighlight>roslaunch myrabot_fer myrabot_lata.launch</syntaxhighlight>

We will execute the next command in a new terminal in order to launch the [[Objects recognition and position calculation (webcam)#Tracking objects|tracking objects]] program:

<syntaxhighlight>roslaunch brazo_fer seguir.launch</syntaxhighlight>

We will execute the next command in a new terminal in order to publish the object to tracking in the ''topic'' "selected_object":

<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>

We will execute the next command in a new terminal in order to launch the "mover_objetos" program with the parameter "modelo_objeto" equal to "lata_amstel":

<syntaxhighlight>rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel</syntaxhighlight>

The arm follows the selected object. We can see this in the next video:

<center><videoflash>kLfgfHo8OnI</videoflash></center>

<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Watch in YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

| valign="top" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]] 
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

MYRAbot's arm model for simulation (urdf+gazebo)

2014-09-29T11:08:51Z

Fernando:

Objects recognition and position calculation (webcam)

2014-09-29T11:08:17Z

Fernando:

MYRAbot's arm control (bioloid+arduino)

2014-09-29T11:07:05Z

Fernando:

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

| valign="top" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]] 
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Bioloid arm=

The arm that we will use is that Carlos Rodríguez Hernández developed in the project [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]]. The main components are:
[[file:IMG_4871_b.JPG|thumb|200px|Photograph of [http://www.robotis.com/xe/BIOLOID_main_en Bioloid] arm.]]

* [http://arduino.cc/en/Main/ArduinoBoardMega2560 Arduino mega 2560] board.
* [http://www.futurlec.com/74LS/74LS241.shtml 74LS241] chip (tree-state Buffer).
* Voltage regulator circuit.
* 5 serial servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A].
* Mounting components of Bioloid kit.
* Parts made for the gripper and to fix to MYRAbot.

=Arduino IDE and rosserial=

For the communication among [http://www.arduino.cc arduino] and [http://www.ros.org ROS], we install [http://www.arduino.cc arduino] IDE and rosserial (''package'' de [http://www.ros.org ROS] which include the ''package'' rosserial_arduino with the libraries for [http://www.arduino.cc arduino]. We will start to installing [http://www.arduino.cc arduino] IDE, for this, we will execute the next commands in a terminal:

<syntaxhighlight>sudo apt-get update
sudo apt-get install arduino arduino-core</syntaxhighlight>

When the installation of [http://www.arduino.cc arduino] software ends, we will install the [http://www.ros.org ROS] ''package''. For this, we will execute the next command in a terminal:

<syntaxhighlight>sudo apt-get install ros-ROS_DISTRO-rosserial</syntaxhighlight>

When we will have installed [http://www.arduino.cc arduino] IDE and the ''stack'' rosserial, we must copy the libraries for the ''package'' rosserial_arduino to the sketchbook of [http://www.arduino.cc arduino]. For this, we will execute the next commands in a terminal:

<syntaxhighlight>roscd rosserial_arduino/libraries
cp -r ros_lib /home/”SESSION_NAME”/sketchbook/libraries/ros_lib</syntaxhighlight>

==Creating a package for our programs==

Previously, we must have created a [[Fernando-TFM-ROS02#Creando un espacio de trabajo|workspace]] for our [http://www.ros.org ROS] distribution. We will create a ''package'' in order to compile and send our programs to [http://www.arduino.cc arduino] board. We will make a ''package'' named "arduino_fer" with the the necessary dependences for our programs, for this, we will execute the next commands in a terminal:

<syntaxhighlight>cd ~/ros_workspace
roscreate-pkg arduino_fer rosserial_arduino std_msgs</syntaxhighlight>

We must change the content of the file "CMakeLists.txt" of our ''package'' for the next lines:

<syntaxhighlight lang=cmake>cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_find_ros_package(rosserial_arduino)
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)</syntaxhighlight>

We will execute the next commands in a terminal in order to finalize the creation of the package:

<syntaxhighlight>roscd arduino_fer
cmake .</syntaxhighlight>

==First program (To whole power)==

We will make the first program that publishes a ''topic'' named "cifra" and subscribe a ''topic'' named "resultado". When we publish a number in the ''topic'' "cifra" the program multiplies the number by itself and publishes the result in the ''topic'' "resultado". The code of the program is shown below:

<syntaxhighlight lang=cpp>
#include <ros.h>
#include <std_msgs/Int16.h>

ros::NodeHandle nh;
int pot;

void potencia( const std_msgs::Int16& cifra){

::pot = cifra.data*cifra.data;
}

ros::Subscriber<std_msgs::Int16> sub("cifra", &potencia );
std_msgs::Int16 res;

ros::Publisher pub("resultado", &res);

void setup()
{
nh.initNode();
nh.subscribe(sub);
nh.advertise(pub);
}

void loop()
{
res.data = ::pot;
pub.publish( &res );
nh.spinOnce();
delay(1000);
}
</syntaxhighlight>

We must add to the file "CMakeLists.txt" the lines indicated below in order in order to compile the program and send to [http://www.arduino.cc arduino] board:

<syntaxhighlight lang=cmake>set(FIRMWARE_NAME potencia)

set(${FIRMWARE_NAME}_BOARD MODELO_NUESTRA_PLACA) # Model of arduino board
set(${FIRMWARE_NAME}_SRCS src/potencia.cpp )
set(${FIRMWARE_NAME}_PORT /dev/ttyACM0) # Serial port to upload
generate_ros_firmware(${FIRMWARE_NAME})</syntaxhighlight>

Here is set the name for the program, type of [http://www.arduino.cc arduino] board (uno, atmega328 o mega2560), name and address of the file, and serial port of the PC used to communicate with the board. We have to execute the next commands in a terminal:

<syntaxhighlight>roscd arduino_fer
make potencia-upload</syntaxhighlight>

We must launch the [http://www.ros.org ROS] core in a terminal in order to allow the execution of the program, if it is not already launched. We will execute the next command in a terminal:

<syntaxhighlight>roscore</syntaxhighlight>

In other terminal, we will execute the node that communicates [http://www.ros.org ROS] with the [http://www.arduino.cc arduino] board, we set the port that is used to communication. In order to know the port that is used, we can see the port used in the menu "Tools">"Serial port" of the software [http://www.arduino.cc arduino] IDE, the board must be plugged. In this case the port is "ttyACM0".

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute in other terminal the next command in order to publish a number in the ''topic'' "cifra":

<syntaxhighlight>rostopic pub cifra std_msgs/Int16 NUMBER --once</syntaxhighlight>

We will execute in other terminal the next command in order to test that the program calculates the square of the published number in the ''topic'' "cifra" and publishes the result in the ''topic'' "resultado":

<syntaxhighlight>rostopic echo resultado</syntaxhighlight>

=Arduino and servo motors Dinamixel=

In [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] we can find the hardware adaptations to communicate the [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] board with the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] of [http://www.robotis.com/xe/ ROBOTICS]. Here also we can find the libraries for programming ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).

==Second program (Move)==

This program is a test of the communications between [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] board and servo motor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. The program publishes the ''topic'' "angulo" which contains the position of the servo motor (between 0 and 1023) and subscribes the ''topic'' "giro" which receives the goal position (between 0 and 1023). The servo motor will move with a constant velocity of 128 (Integer between 0 and 1023). The code of the program is shown below:

<syntaxhighlight lang=cpp>
#include <ros.h>
#include <std_msgs/Int16.h>
#include <DynamixelSerial1.h>

ros::NodeHandle nh;

void mover( const std_msgs::Int16& giro){

Dynamixel.moveSpeed(1,giro.data,128);

}

ros::Subscriber<std_msgs::Int16> sub("giro", &mover );

std_msgs::Int16 ang;
ros::Publisher pub("angulo", &ang);

void setup()
{
nh.initNode();
nh.subscribe(sub);
nh.advertise(pub);
Dynamixel.begin(1000000,2);
delay(1000);
}

void loop()
{
int posicion = Dynamixel.readPosition(1);
ang.data = posicion;
pub.publish( &ang );
nh.spinOnce();
delay(10);
}
</syntaxhighlight>

We can't use the [[#Creating a package for our programs|previously made ''package'']] in order to compile and send the program to the [http://www.arduino.cc arduino] board. We must use the [http://www.arduino.cc arduino] IDE software due to that we use a external library to [http://www.ros.org ROS] system. When the program was uploaded we will execute the next command in a terminal, to launch the core system:

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in other terminal in order to publish the goal position of the servo motor in the ''topic'' "giro":

<syntaxhighlight>rostopic pub giro std_msgs/Int16 GOAL_POSITION --once</syntaxhighlight>

The servo motor will immediately move to the goal position. We can execute the next command in other terminal in order to see the position value of the servo motor at any time:

<syntaxhighlight>rostopic echo angulo</syntaxhighlight>

=Arm's kinematics model=

We need to know the spatial position of the arm for the correct placement of the gripper. The position depends on the position angle of the servo motors (direct kinematics). We know the goal point and we must obtain the position angle of the servo motors to achieve it (inverse kinematics). The arm have 5 joints (1 to open-close gripper) and all work in the same plane, for this reason we don't need use the [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters Denavit-Hartenberg parameters].

[[file:brazo_cinematica.jpg|thumb|500px|center|Diagrams for kinematics study]]

==Inverse kinematic==

We use the diagram shown above to obtain the inverse kinematics. Where ''x'', ''y'' and ''z'' are the goal point coordinates, and ε is the angle that the gripper forms with x-z plane. The obtained equations are shown below:

[[file:brazo_ecuaciones.jpg]]

We have to calculate the value of position angle for the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] according its turn limits (1023 positions throughout 300º):

[[file:brazo_dynamixel_AX-12.jpg|thumb|250px|Diagrama de ángulos servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]]]

[[file:brazo_ecuacion_base.jpg]] We add 150º from the position angle in order to set the 0º coincident with z axis.

[[file:brazo_ecuacion_arti1.jpg]] We add 60º from the position angle in order to set the 0º coincident with z axis.

[[file:brazo_ecuacion_arti2.jpg]] We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.

[[file:brazo_ecuacion_arti3.jpg]] We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.

==Direct kinematics==

We can use the previous equations to obtain the placement point through the position angle of the servo motors. The equations are shown below:

[[file:brazo_ecuaciones_directa.jpg]]

==Arm workspace and boundaries==

We need to set the boundaries of the position angles of the servo motors to avoid self collisions and collision with other parts of MYRAbot. We have made a spreadsheet using the equations for inverse and direct kinematics. The spreadsheet shows the spacial placement of the arm in two dispersion graphs.

[[file:brazo_cinematica_excel.jpg|thumb|750px|center|Spreadsheet of inverse and direct kinematics, [http://www.fernando.casadogarcia.es/files/cinematica_brazo_MYRAbot.xlsx download]]]

=Control programs=

==Third program (Come here)==

We place a interface program in [http://www.arduino.cc arduino] board that communicates the servo motor with the control programs in [http://www.ros.org ROS]. We need publish and subscribe the data of 5 servo motors so we will create own messages.

===Creating custom messages in ROS===

First, we will create a new ''package'' with the necessary dependences for our programs (std_msgs geometry_msgs roscpp). We will execute the next commands in a terminal:

<syntaxhighlight>
cd ~/ros_workspace
roscreate-pkg brazo_fer std_msgs geometry_msgs roscpp</syntaxhighlight>

We will create a folder named "msg" within the created ''package'', here we place the next description files for our messages (data type and field name).

We will create the basis message that contains the data for each servo motor. We will create a file named "Servos.msg" with the content that is shown below:

<syntaxhighlight>int16 base
int16 arti1
int16 arti2
int16 arti3
int16 pinza</syntaxhighlight>

We will use the previous message in order to define the data type. We will create a file named "WriteServos.msg" to send data to the servo motors (position, velocity and torque) with the content that is shown below:

<syntaxhighlight>Servos posicion
Servos velocidad
Servos par</syntaxhighlight>

We will create a file named "ReadServos.msg" in order to receive data to the servo motors (position, status and current) with the content that is shown below:

<syntaxhighlight>Servos posicion
Servos estado
Servos corriente</syntaxhighlight>

We have to edit the file "CMakeLists.tx" in our ''package'' in order to allow the creation the [http://www.ros.org ROS] messages. We will delete the symbol "#" in the line that is shown below:

<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the messages, where "brazo_fer" is the name of the created ''package'':

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

We will execute the next command in a terminal in order to add this messages to the [http://www.arduino.cc arduino] libraries in "rosserial":

<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>

===Arduino program===

The program for [http://www.arduino.cc arduino] board subscribes the ''topic'' "move_arm" which receives the orders for the servo motors and publishes the "topic" "pose_arm" which sends the servo motors information. The code of the program is shown below:

<syntaxhighlight lang="cpp">

#include <ros.h>
#include <brazo_fer/Servos.h>
#include <brazo_fer/WriteServos.h>
#include <brazo_fer/ReadServos.h>
#include <std_msgs/Bool.h>
#include <math.h>

#include <DynamixelSerial1.h>

ros::NodeHandle nh;

void mover(const brazo_fer::WriteServos& brazo){

brazo_fer::Servos par = brazo.par;

brazo_fer::Servos vel = brazo.velocidad;

brazo_fer::Servos move = brazo.posicion;

int posicion[4];

if (par.base == 0){
Dynamixel.torqueStatus(1,OFF);
}
else {
Dynamixel.torqueStatus(1,ON);

Dynamixel.moveSpeed(1,move.base,vel.base);
}

if (par.arti1 == 0){
Dynamixel.torqueStatus(2,OFF);
}
else {
Dynamixel.torqueStatus(2,ON);

Dynamixel.moveSpeed(2,move.arti1,vel.arti1);
}

if (par.arti2 == 0){
Dynamixel.torqueStatus(3,OFF);
}
else {
Dynamixel.torqueStatus(3,ON);

Dynamixel.moveSpeed(3,move.arti2,vel.arti2);
}

if (par.arti3 == 0){
Dynamixel.torqueStatus(4,OFF);
}
else {
Dynamixel.torqueStatus(4,ON);

Dynamixel.moveSpeed(4,move.arti3,vel.arti3);
}

}

void pinza(const brazo_fer::WriteServos& pinza){

brazo_fer::Servos par = pinza.par;

brazo_fer::Servos vel = pinza.velocidad;

brazo_fer::Servos move = pinza.posicion;

int posicion;

if (par.pinza == 0){
Dynamixel.torqueStatus(5,OFF);
}
else {
Dynamixel.torqueStatus(5,ON);

Dynamixel.moveSpeed(5,move.pinza,vel.pinza);
}

}

ros::Subscriber<brazo_fer::WriteServos> move_sub("move_arm", &mover );
ros::Subscriber<brazo_fer::WriteServos> hand_sub("hand_arm", &pinza );

brazo_fer::ReadServos pec;
std_msgs::Bool pulsador;

ros::Publisher pose_pub("pose_arm", &pec);

void setup()
{
nh.initNode();
nh.subscribe(move_sub);
nh.subscribe(hand_sub);
nh.advertise(pose_pub);

Dynamixel.begin(1000000,2);

}

void loop()
{
brazo_fer::Servos pos;
brazo_fer::Servos est;
brazo_fer::Servos cor;

pos.base = Dynamixel.readPosition(1);
pos.arti1 = Dynamixel.readPosition(2);
pos.arti2 = Dynamixel.readPosition(3);
pos.arti3 = Dynamixel.readPosition(4);
pos.pinza = Dynamixel.readPosition(5);

est.base = Dynamixel.moving(1);
est.arti1 = Dynamixel.moving(2);
est.arti2 = Dynamixel.moving(3);
est.arti3 = Dynamixel.moving(4);
est.pinza = Dynamixel.moving(5);

cor.base = Dynamixel.readLoad(1);
cor.arti1 = Dynamixel.readLoad(2);
cor.arti2 = Dynamixel.readLoad(3);
cor.arti3 = Dynamixel.readLoad(4);
cor.pinza = Dynamixel.readLoad(5);

pec.posicion = pos;
pec.estado = est;
pec.corriente = cor;

pose_pub.publish( &pec );

nh.spinOnce();

}

</syntaxhighlight>

We will use [http://www.arduino.cc arduino] IDE software to compile and send the program to the board. We will use this program for the next control programs since it is a interface between the servo motors and [http://www.ros.org ROS] system.

===ROS program===

This program uses the equations of the inverse kinematics to set the position angle of the servo motors in order to reach the goal point that is within the workspace. The program subscribe the ''topic'' "point" which receives the goal point, also it subscribes the ''topic'' "pose_arm" which receives the information of servo motors, and publish the ''topic' "move_arm" which indicates to the [http://www.arduino.cc arduino] program the position, velocity and torque of for the servo motors. We will create a file named "control_v01.cpp" with the content that is shown below within the folder "src" of our ''package'':

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "geometry_msgs/Point.h"
#include "math.h"

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

brazo_fer::Servos move, vel, p, e, c;

void posicion(const brazo_fer::ReadServos& pose)
{

::p = pose.posicion;
::e = pose.estado;
::c = pose.corriente;

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

double x = point.x;
double y = point.y;
double z = point.z;

y = y + z*tan(atan2(45,250));

int coordenadas_correctas = 1;

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
double z_p;
double L_a, L;

epsilon = 0;

alfa = (atan2(x,z)*180)/PI;

z_p = sqrt(pow(z,2)+pow(x,2));

L = sqrt(pow(z_p,2)+pow(y,2));

L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));

beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta = ((beta_p+beta_a)*180)/PI;

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

delta_a = PI-(beta_a+gamma);

gamma = (gamma*180)/PI;

delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (beta_pp > beta_p) {
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
}
else {
delta = ((delta_p+delta_a)*180)/PI;

if (isnan(delta)) {
delta = ((PI+delta_a)*180)/PI;
}
}

if (isnan(gamma)) // si no hay solución
{
coordenadas_correctas = 0;
}

::move.base = ((alfa+150)*1023)/300;
::move.arti1 = ((beta+60)*1023)/300;
::move.arti2 = ((gamma-30)*1023)/300;
::move.arti3 = ((delta-30)*1023)/300;
::move.pinza = 511;

::vel.base = abs(::move.base - ::p.base)/5;
::vel.arti1 = abs(::move.arti1 - ::p.arti1)/5;
::vel.arti2 = abs(::move.arti2 - ::p.arti2)/5;
::vel.arti3 = abs(::move.arti3 - ::p.arti3)/5;
::vel.pinza = abs(::move.pinza - ::p.pinza);

if (coordenadas_correctas == 1 && (205 <= ::move.base && ::move.base <= 818) && (120 <= ::move.arti1 && ::move.arti1 <= 920) && ::move.arti2 >= 50 && ::move.arti3 <= 828) {

brazo_fer::WriteServos mover;

mover.posicion = ::move;

mover.velocidad = ::vel;

mover.par.base = 1;
mover.par.arti1 = 1;
mover.par.arti2 = 1;
mover.par.arti3 = 1;
mover.par.pinza = 1;

move_pub_.publish(mover);
}
else {
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
}

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_brazo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);

ros::Subscriber point_sub_= n.subscribe("point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our ''package'' in order to compile and create the executable file:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v01 src/control_v01.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Running the program===

First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in a terminal in order to launch the control program:

<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>

We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal point:

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>

The arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:

<center><videoflash>iwAcutO3C8g</videoflash></center>

<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Watch in YouTube]</center>

==Fourth program (Grasp the bottle)==

===Include file with functions===

We have created a file with the control fuctions for the arm in orther to simplify and make more legible the programs. We will create a file named "brazo_fer.h" within the folder "include/brazo_fer" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang="cpp" enclose="div">

#ifndef brazo_fer_H
#define brazo_fer_H

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "std_msgs/Int16.h"
#include "geometry_msgs/Point.h"
#include "math.h"

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

geometry_msgs::Point directa(brazo_fer::Servos posicion_servos_0, int inclinacion_pinza)
{

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p1, delta_p2, epsilon;
double z_p;
double L_a, L;
double a, b;
double x, y, z;

alfa = (((posicion_servos_0.base*300)/1023)-150)*PI/180;
beta = (((posicion_servos_0.arti1*300)/1023)-60)*PI/180;
gamma = (((posicion_servos_0.arti2*300)/1023)+30)*PI/180;
delta = (((posicion_servos_0.arti3*300)/1023)+30)*PI/180;

epsilon = (inclinacion_pinza*PI)/180;

L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta_p = beta - beta_a;

delta_a = PI - (beta_a + gamma);

delta_p1 = delta - delta_a;

delta_p2 = 2*PI - (delta - delta_a);

if (delta_p1 < delta_p2)
{
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p1));
}
else
{
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p2));
}

z_p = L_a*cos(beta_p) + Lp*cos(epsilon);

beta_pp = acos(z_p/L);

a = L1*sin(beta);

b = L2*sin(beta+gamma)+Lp*sin(epsilon);

if (a >= b)
{
y = L*sin(beta_pp);
}
else
{
y = -L*sin(beta_pp);
}

z = z_p*cos(alfa);

x = z_p*sin(alfa);

geometry_msgs::Point punto;

punto.x = x;
punto.y = y;
punto.z = z;

return punto;
}

brazo_fer::WriteServos inversa(geometry_msgs::Point destino, int inclinacion_pinza, brazo_fer::Servos posicion_servos_0, int velocidad)
{

double x = destino.x;
double y = destino.y;
double z = destino.z;

int coordenadas_correctas = 1;

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
double z_p;
double L_a, L;

epsilon = (inclinacion_pinza*PI)/180;

alfa = (atan2(x,z)*180)/PI;

z_p = sqrt(pow(z,2)+pow(x,2));

L = sqrt(pow(z_p,2)+pow(y,2));

L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));

beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta = ((beta_p+beta_a)*180)/PI;

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

delta_a = PI-(beta_a+gamma);

gamma = (gamma*180)/PI;

delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (beta_pp > beta_p) {
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
}
else {
delta = ((delta_p+delta_a)*180)/PI;

if (isnan(delta)) {
delta = ((PI+delta_a)*180)/PI;
}
}

if (isnan(gamma))
{
coordenadas_correctas = 0;
}

brazo_fer::Servos posicion_servos_1;
brazo_fer::Servos velocidad_servos;
brazo_fer::Servos par_servos;

posicion_servos_1.base = ((alfa+150)*1023)/300;
posicion_servos_1.arti1 = ((beta+60)*1023)/300;
posicion_servos_1.arti2 = ((gamma-30)*1023)/300;
posicion_servos_1.arti3 = ((delta-30)*1023)/300;

if (velocidad == 0)
{
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)/5;
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)/5;
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)/5;
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)/5;
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}

}
else
{
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)*(velocidad/10);
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)*(velocidad/10);
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)*(velocidad/10);
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)*(velocidad/10);
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}

}

brazo_fer::Servos velocidad_servos_0;

velocidad_servos_0.base = 0;
velocidad_servos_0.arti1 = 0;
velocidad_servos_0.arti2 = 0;
velocidad_servos_0.arti3 = 0;

par_servos.base = 1;
par_servos.arti1 = 1;
par_servos.arti2 = 1;
par_servos.arti3 = 1;

brazo_fer::WriteServos move_arm;

if (coordenadas_correctas == 1 && (205 <= posicion_servos_1.base && posicion_servos_1.base <= 818) && (120 <= posicion_servos_1.arti1 && posicion_servos_1.arti1 <= 920) && posicion_servos_1.arti2 >= 50 && (posicion_servos_1.arti3 <= 828 && posicion_servos_1.arti3 >= 195)) {
move_arm.posicion = posicion_servos_1;
move_arm.velocidad = velocidad_servos;
move_arm.par = par_servos;
return move_arm;
}
else {
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
move_arm.posicion = posicion_servos_0;
move_arm.velocidad = velocidad_servos_0;
move_arm.par = par_servos;
return move_arm;
}
}

brazo_fer::WriteServos control_pinza(brazo_fer::Servos posicion_servos_1, brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
{

brazo_fer::Servos velocidad_servos;
brazo_fer::Servos par_servos;

velocidad_servos.pinza = 50;

par_servos.pinza = 1;

brazo_fer::WriteServos hand_arm;

if ((posicion_servos_1.pinza >= 480 && posicion_servos_1.pinza <= 680 && corriente_servos.pinza <= 300) || (posicion_servos_0.pinza > posicion_servos_1.pinza && posicion_servos_1.pinza >= 480))
{
hand_arm.posicion = posicion_servos_1;
hand_arm.velocidad = velocidad_servos;
hand_arm.par = par_servos;
return hand_arm;
}
else
{
std::cout<<"Alcanzado límite de la pinza"<<std::endl;
hand_arm.posicion = posicion_servos_0;
hand_arm.velocidad = velocidad_servos;
hand_arm.par = par_servos;
return hand_arm;
}
}

geometry_msgs::Point home(brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

geometry_msgs::Point punto_0;

punto_0.x = 0;
punto_0.y = 80;
punto_0.z = 50;

int inclinacion_pinza = 0;

brazo_fer::WriteServos inicio_brazo = inversa(punto_0, inclinacion_pinza, posicion_servos_0, 0);

brazo_fer::Servos posicion_servos_1;

posicion_servos_1.pinza = 511;

brazo_fer::WriteServos inicio_pinza = control_pinza(posicion_servos_1, posicion_servos_0, corriente_servos);

move_pub_.publish(inicio_brazo);

hand_pub_.publish(inicio_pinza);

return punto_0;

}

#endif

</syntaxhighlight>

===Program===

This program execute the sequence to grasp a little plastic bottle (the bottle is empty since the torque of the servo motors is low, we need more servo motors on each joint in order to pick more load). The sequence steps are shown below:

# Approach (place the gripper at 70 mm of the bottle position).
# Placement in the bottle position.
# To close the gripper (until to hold the bottle).
# Lifting and approach.

We will create a file named "control_v02.cpp" with the content that is shown below within the folder "src" of our ''package'':

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "math.h"

geometry_msgs::Point punto_0,punto_1;

int cont = 0;
int cont_1 = 0;
int cont_2 = 0;
int inclinacion_pinza = 0;
int start = 0;

brazo_fer::Servos pg, cg;
brazo_fer::WriteServos move;
brazo_fer::Servos pinza;
brazo_fer::WriteServos pin;
brazo_fer::WriteServos error_coordenadas;

brazo_fer::WriteServos acercar(geometry_msgs::Point cerca)
{
double x = cerca.x;
double z = cerca.z;

cerca.z = cerca.z - 70*cos(atan2(x,z));
cerca.x = cerca.x - 70*sin(atan2(x,z));

brazo_fer::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);

return move;

}

brazo_fer::WriteServos levantar_acercar(geometry_msgs::Point la)
{

la.x = 0;
la.y = la.y + 50;
la.z = 60;

if (la.y < 75) {
la.y = 75;
}

brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);

::punto_0 = la;

return move;

}

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

brazo_fer::Servos p = pec.posicion;

::pg = pec.posicion;
::cg = pec.corriente;

brazo_fer::Servos e = pec.estado;

brazo_fer::Servos c = pec.corriente;

if (::cont == 0)
{
::punto_0 = home(p, c);

::punto_1 = ::punto_0;

::cont = ::cont+1;

}

if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {

if (::cont_1 == 0) {

::move = acercar(::punto_0);

move_pub_.publish(::move);

::cont_1 = ::cont_1+1;

}

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

if (::cont_2 == 0) {

::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);

move_pub_.publish(::move);

::cont_2 = ::cont_2+1;

}
else {

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

::pinza.pinza = p.pinza;

if(::pin.posicion.pinza != p.pinza) {

std::cout<<c.pinza<<std::endl;

::pinza.pinza = ::pinza.pinza + 20;

::pin = control_pinza(::pinza, p, c);

hand_pub_.publish(::pin);

}
else {

std::cout<<"Agarrado"<<std::endl;

::move = levantar_acercar(::punto_0);

move_pub_.publish(::move);

::punto_1 = ::punto_0;

::cont_1 = 0;
::cont_2 = 0;
}
}
}

}
else {

std::cout<<"En movimiento"<<std::endl;
}

}

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); //Publicación del topic "move_arm"

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); //Publicación del topic "hand_arm"

::punto_0 = point;

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_recogida");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Running the program===

First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in a terminal in order to launch the control program:

<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>

We will execute the next command in a terminal in order to publish in the ''topic'' "pick_point" the bottle position (We have placed the bottle in a known position but we can use a vision system to set the bottle position):

<syntaxhighlight>rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>

The arm will execute the pick sequence. We can see the arm grasping the bottle in the next video:

<center><videoflash>0FcB6-i23cU</videoflash></center>

<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Watch in YouTube]</center>

==Fifth program (Place the bottle)==

This program execute the sequence to place the bottle. The sequence steps are shown below:

# Placement in the goal position.
# To open the gripper (until to free the bottle).
# Distance (place the gripper at 70 mm of the goal position).
# Return to initial position.

We will create a file named "control_v03.cpp" with the content that is shown below within the folder "src" of our package:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "math.h"

geometry_msgs::Point punto_0, punto_1;

int cont = 0;
int cont_1 = 0;
int cont_2 = 0;
int cont_3 = 0;
int inclinacion_pinza = 0;

brazo_fer::Servos pg, cg, pinza;
brazo_fer::WriteServos move;
brazo_fer::WriteServos pin;
brazo_fer::WriteServos error_coordenadas;

brazo_fer::WriteServos separar(geometry_msgs::Point lejos)
{
double x = lejos.x;
double z = lejos.z;

lejos.z = lejos.z - 70*cos(atan2(x,z));
lejos.x = lejos.x - 70*sin(atan2(x,z));

brazo_fer::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);

return move;

}

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

brazo_fer::Servos p = pec.posicion;

::pg = pec.posicion;
::cg = pec.corriente;

brazo_fer::Servos e = pec.estado;

brazo_fer::Servos c = pec.corriente;

if (::cont == 0)
{
::punto_0 = directa(p, inclinacion_pinza);

::punto_1 = ::punto_0;

::cont = ::cont+1;
}

if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {

if (::cont_1 == 0) {

::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);

move_pub_.publish(::move);

::cont_1 = ::cont_1+1;

}

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

if (::cont_2 == 0) {

::pinza.pinza = 470;

::pin = control_pinza(::pinza, p, c);

hand_pub_.publish(::pin);

::cont_2 = ::cont_2+1;

}
else {

if (p.pinza <= 495) {

if (::cont_3 == 0) {

::move = separar(::punto_0);

move_pub_.publish(::move);

::cont_3 = ::cont_3+1;

}

else if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

std::cout<<"Suelto"<<std::endl;

::cont = 0;

::cont_1 = 0;
::cont_2 = 0;
::cont_3 = 0;
}
}
}

}
else {

std::cout<<"En movimiento"<<std::endl;
}
}

}

void punto(const geometry_msgs::Point& point)
{

::punto_0 = point;

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_entrega");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Running the program===

First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in a terminal in order to launch the control program:

<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>

We will execute the next command in a terminal in order to publish in the ''topic'' "place_point" the goal position:

<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>

==Sixth program (Follow the straight way)==

This program move the arm following the line between the initial position and the goal position. We will create a file named "control_v04.cpp" with the content that is shown below within the folder "src" of our package:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "geometry_msgs/Point.h"
#include "math.h"

brazo_fer::Servos p, e, c;
brazo_fer::WriteServos destino, mover;
geometry_msgs::Point punto_0, punto_1, punto_i;
double lambda = 0;
int inclinacion_pinza = 0;
int cont = 0;

void posicion(const brazo_fer::ReadServos& pose)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

::p = pose.posicion;
::e = pose.estado;
::c = pose.corriente;

if (cont == 0)
{
::punto_0 = home(::p, ::c);

::punto_1 = ::punto_0;

cont = 1;
}

if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::mover.posicion.base && ::mover.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::mover.posicion.arti1 && ::mover.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::mover.posicion.arti2 && ::mover.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::mover.posicion.arti3 && ::mover.posicion.arti3 < (::p.arti3+15)))
{

if (((::p.base-15) < ::destino.posicion.base && ::destino.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.posicion.arti1 && ::destino.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.posicion.arti2 && ::destino.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.posicion.arti3 && ::destino.posicion.arti3 < (::p.arti3+15)))
{
::punto_0 = ::punto_1;
::lambda = 0;
}
else
{
geometry_msgs::Point u;

//ecuación de la recta p1 = p0+lambda.u

u.x = ::punto_1.x - ::punto_0.x;
u.y = ::punto_1.y - ::punto_0.y;
u.z = ::punto_1.z - ::punto_0.z;

double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);

::lambda = ::lambda + 1/paso;

::punto_i.x = ::punto_0.x + (::lambda)*u.x;
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
::punto_i.z = ::punto_0.z + (::lambda)*u.z;

::mover = inversa(::punto_i, ::inclinacion_pinza, ::p, 0);

move_pub_.publish(::mover);

}

}

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

::lambda = 0;

::punto_0 = directa(::p, ::inclinacion_pinza);

::punto_1 = point;

::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_brazo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);

ros::Subscriber point_sub_= n.subscribe("point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Running the program===

First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in a terminal in order to launch the control program:

<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>

We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal position:

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>

==Seventh program (Teleoperator)==

This program allows to control the arm using the keyboard of the remote PC. We will create a file named "teleoperador_teclado.cpp" with the content that is shown below within the folder "src" of our package:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "geometry_msgs/Point.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_Xmas 0x43
#define KEYCODE_Xmenos 0x44
#define KEYCODE_Ymas 0x41
#define KEYCODE_Ymenos 0x42
#define KEYCODE_Zmas 0x77
#define KEYCODE_Zmenos 0x73
#define KEYCODE_Pabrir 0x61
#define KEYCODE_Pcerrar 0x64
#define KEYCODE_PinclinarMas 0x65
#define KEYCODE_PinclinarMenos 0x71

struct termios cooked, raw;

int cont = 0;

double retardo = 0.11;

geometry_msgs::Point punto;
brazo_fer::Servos vel;
brazo_fer::Servos pinza, p, c;

int pinza_incli = 0;

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

::p = pec.posicion;
::c = pec.corriente;

}

void quit(int sig)
{
tcsetattr(0, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}

void callback(const ros::TimerEvent&)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::Time anterior;

brazo_fer::WriteServos teleop;
brazo_fer::WriteServos teleop_pinza;

if (::cont == 0)
{
anterior = ros::Time::now();

::punto = home(::p, ::c);

::cont = 1;

::pinza.pinza = 511;

teleop = inversa(::punto, ::pinza_incli, ::p, 0);
teleop_pinza = control_pinza(::pinza, ::p, ::c);

//std::cout<<teleop<<"-"<<teleop_pinza<<std::endl;
}

char c;

// get the console in raw mode
tcgetattr(0, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(0, TCSANOW, &raw);

puts("");
puts("#####################################################");
puts(" ARM CONTROLS");
puts("#####################################################");
puts("");
puts("Up arrow:___________ Move up gripper");
puts("Down arrow:_________ Move down gripper");
puts("Left arrow:_________ Move left gripper");
puts("Right arrow:________ Move right gripper");
puts("W key:______________ Move forward gripper");
puts("S key:______________ Move backward gripper");
puts("A key:______________ Open gripper");
puts("D key:______________ Close gripper");
puts("Q key:______________ Tilt up gripper");
puts("E key:______________ Tilt down gripper");

while (ros::ok())
{

// get the next event from the keyboard
if(read(0, &c, 1) < 0)
{
perror("read():");
exit(-1);
}

if (c == KEYCODE_Xmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.x = ::punto.x - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.x = ::punto.x + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Xmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.x = ::punto.x + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.x = ::punto.x - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.y = ::punto.y + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.y = ::punto.y - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.y = ::punto.y - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.y = ::punto.y + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.z = ::punto.z + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.z = ::punto.z - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.z = ::punto.z - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.z = ::punto.z + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Pabrir)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza.pinza = ::pinza.pinza - 5;
teleop_pinza = control_pinza(::pinza, ::p, ::c);
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Pcerrar)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza.pinza = ::pinza.pinza + 5;
teleop_pinza = control_pinza(::pinza, ::p, ::c);
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_PinclinarMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza_incli = ::pinza_incli + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::pinza_incli = ::pinza_incli - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_PinclinarMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza_incli = ::pinza_incli - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::pinza_incli = ::pinza_incli + 5;}
anterior = ros::Time::now();
}
}

if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
{
move_pub_.publish(teleop);
}
if (teleop_pinza.posicion.pinza != ::p.pinza)
{
hand_pub_.publish(teleop_pinza);
}

}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_teclado");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

signal(SIGINT,quit);

ros::Timer timer = n.createTimer(ros::Duration(1), callback);

ros::spin();

return 0;
}

</syntaxhighlight>

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:

<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>

We will execute the next commands in a terminal in order to compile and create the executable file:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Running the program===

First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.

<syntaxhighlight>roscore</syntaxhighlight>

We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

We will execute the next command in a terminal in order to launch the control program:

<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</syntaxhighlight>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

| valign="top" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]] 
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

2014-09-29T11:06:23Z

Fernando:

Órdenes y confirmación mediante voz (sphinx+festival)

2014-09-29T11:05:58Z

Fernando:

Integración de MYRAbot en moveIt! (gazebo+moveIt!)

2014-09-29T11:04:21Z

Fernando:

Modelo para simulación MYRAbot (urdf+gazebo)

2014-09-29T11:03:50Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Componentes del modelo de MYRAbot=

Se ha comenzado realizado un modelo de la estructura del MYRAbot a la cual se le ha añadido la base [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba], la cámara [http://www.xbox.com/es-ES/Kinect kinect], el [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|modelo del brazo]] y la cámara web [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000]. Hemos comenzado [[Fernando-TFM-ROS02#Creando un package|creando un package]] que va a contener el modelo de nuestro robot, que llamaremos "myrabot_fer_modelo", con las dependencias urdf gazebo sensor_msgs roscpp.

==URDF estructura MYRAbot==

Se ha realizado previamente un modelo 3D de la estructura con un programa [http://es.wikipedia.org/wiki/Dise%C3%B1o_asistido_por_computadora CAD], del que vamos a emplear las partes con una geometria singular, ya que el diseño empleando los elementos geométricos básicos sería complicado.

[[file:estructura_MYRAbot_modelo.jpg|thumb|500px|center|Diseño [http://es.wikipedia.org/wiki/Dise%C3%B1o_asistido_por_computadora CAD] 3D de la estructura del MYRAbot]]

Para el archivo principal crearemos un archivo llamado "estructura-myrabot.xacro" en el directorio "urdf" de el ''package'' anteriormente creado, con el siguiente contenido:

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot>

<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot-macros.xacro" />

<macro name="estructura_myrabot" params="parent *origin">

<joint name="world_joint" type="fixed">
<parent link="${parent}"/>
<child link="e_base_1_link"/>
<insert_block name="origin" />
</joint>

<link name="e_base_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
</collision>
</link>

<separador_base id="1" />
<e_separador_base id="1" xsim="1" ysim="1" />

<separador_base id="2" />
<e_separador_base id="2" xsim="1" ysim="-1" />

<separador_base id="3" />
<e_separador_base id="3" xsim="-1" ysim="-1" />

<separador_base id="4" />
<e_separador_base id="4" xsim="-1" ysim="1" />

<joint name="base_2" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_base_2_link"/>
<origin xyz="0 0 0.135" rpy="0 0 0" />
</joint>

<link name="e_base_2_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
</collision>
</link>

<joint name="pilar" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_pilar_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_pilar_link">
<inertial>
<mass value="0.08"/>
<origin rpy="0 0 0" xyz="0 0 ${0.7/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="ultrasonidos" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_ultrasonidos_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_ultrasonidos_link">
<inertial>
<mass value="0.04"/>
<origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="botonera" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_botonera_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>

<link name="e_botonera_link">
<inertial>
<mass value="0.06"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
</collision>
</link>

<joint name="start_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_start_button_1_link"/>
<origin xyz="-0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_start_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
</collision>
</link>

<joint name="start_button_2" type="fixed">
<parent link="e_start_button_1_link"/>
<child link="e_start_button_2_link"/>
<origin xyz="0.0 0 0.002" rpy="0 0 0" />
</joint>

<link name="e_start_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
</collision>
</link>

<joint name="emergency_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_emergency_button_1_link"/>
<origin xyz="0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
</collision>
</link>

<joint name="emergency_button_2" type="fixed">
<parent link="e_emergency_button_1_link"/>
<child link="e_emergency_button_2_link"/>
<origin xyz="0.0 0 0.015" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
</collision>
</link>

<joint name="base_kinect" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_kinect_link"/>
<origin xyz="0 0 ${0.395-0.155}" rpy="0 0 0" />
</joint>

<link name="e_base_kinect_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="base_brazo_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_brazo_1_link"/>
<origin xyz="0 0 ${0.708-(0.395-0.155)}" rpy="0 0 0" />
</joint>

<link name="e_base_brazo_1_link">
<inertial>
<mass value="0.03"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<separador_base_brazo id="1" />
<e_separador_base_brazo id="1" xsim="1" ysim="1" />

<separador_base_brazo id="2" />
<e_separador_base_brazo id="2" xsim="1" ysim="-1" />

<separador_base_brazo id="3" />
<e_separador_base_brazo id="3" xsim="-1" ysim="-1" />

<separador_base_brazo id="4" />
<e_separador_base_brazo id="4" xsim="-1" ysim="1" />

<joint name="base_brazo_2" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_base_brazo_2_link"/>
<origin xyz="0 0 0.074" rpy="0 0 0" />
</joint>

<link name="e_base_brazo_2_link">
<inertial>
<mass value="0.005"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="soporte_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_soporte_1_link"/>
<origin xyz="0 0 ${0.995-(0.708-0.155)-(0.395-0.155)}" rpy="0 0 0" />
</joint>

<link name="e_soporte_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0 0 ${0.64/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="soporte_2" type="fixed">
<parent link="e_soporte_1_link"/>
<child link="e_soporte_2_link"/>
<origin xyz="0 0.2 0.61" rpy="-0.12 0 0" />
</joint>

<link name="e_soporte_2_link">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 ${0.06/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="monitor" type="fixed">
<parent link="e_soporte_2_link"/>
<child link="e_monitor_link"/>
<origin xyz="0 -0.2 -0.09" rpy="0 0 0" />
</joint>

<link name="e_monitor_link">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0 0 ${0.180/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="pantalla" type="fixed">
<parent link="e_monitor_link"/>
<child link="e_pantalla_link"/>
<origin xyz="0.135 ${0.135-0.005-0.03-0.0001} 0.086" rpy="0 0 0" />
</joint>

<link name="e_pantalla_link">
<inertial>
<mass value="0.0001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.130" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.127" />
</geometry>
</collision>
</link>

<gazebo_propiedades nombre="e_base_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_separador_base_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_4_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_4_link" material="Grey" />

<gazebo_propiedades nombre="e_pilar_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_kinect_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_botonera_link" material="LightWood" />

<gazebo_propiedades nombre="e_start_button_1_link" material="Black" />

<gazebo_propiedades nombre="e_start_button_2_link" material="Green" />

<gazebo_propiedades nombre="e_emergency_button_1_link" material="Grey" />

<gazebo_propiedades nombre="e_emergency_button_2_link" material="Red" />

<gazebo_propiedades nombre="e_soporte_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_soporte_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_monitor_link" material="LightWood" />

<gazebo_propiedades nombre="e_pantalla_link" material="Black" />

</macro>

</robot>

</syntaxhighlight>

Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la estructura del robot se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro ''package'':

* [http://www.fernando.casadogarcia.es/files/e_pilar.stl e_pilar.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_kinect.stl e_base_kinect.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_brazo_1.stl e_base_brazo_1.stl]
* [http://www.fernando.casadogarcia.es/files/e_base_brazo_2.stl e_base_brazo_2.stl]
* [http://www.fernando.casadogarcia.es/files/e_soporte_1.stl e_soporte_1.stl]
* [http://www.fernando.casadogarcia.es/files/e_soporte_2.stl e_soporte_2.stl]
* [http://www.fernando.casadogarcia.es/files/e_monitor.stl e_monitor.stl]
* [http://www.fernando.casadogarcia.es/files/e_ultrasonidos.stl e_ultrasonidos.stl]

Los macros que estamos usando en el archivo principal del [http://wiki.ros.org/urdf/Tutorials URDF] son los que se muestran continuación, que guardaremos en un archivo llamado "estructura-myrabot-macros.xacro" en el directorio "urdf" de nuestro ''package'':

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot>

<macro name="default_inertia_e">
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>

<macro name="separador_base" params="id">
<joint name="separador_base_${id}" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_separador_base_${id}_link"/>
<origin xyz="0.135 0.135 0.015" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base" params="id xsim ysim">
<link name="e_separador_base_${id}_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
<material name="metal">
<color rgba="${211/255} ${211/255} ${211/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
</collision>
</link>
</macro>

<macro name="separador_base_brazo" params="id">
<joint name="separador_base_brazo_${id}" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_separador_base_brazo_${id}_link"/>
<origin xyz="0.135 ${0.135-0.082} 0.075" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base_brazo" params="id xsim ysim">
<link name="e_separador_base_brazo_${id}_link">
<inertial>
<mass value="0.002"/>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
</collision>
</link>
</macro>

<macro name="gazebo_propiedades" params="nombre material">
<gazebo reference="${nombre}">
<material>Gazebo/${material}</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</macro>

</robot>

</syntaxhighlight>

==URDF Roomba==

Para la base móvil [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba] se ha empleado el modelo del [http://www.turtlebot.com/ turtlebot], aunque equipado con una base móvil [http://store.irobot.com/family/index.jsp?categoryId=2591511&s=A-ProductAge create], esta tiene unas físicas y sensores similares a la nuestra. Para esto necesitamos instalar los ''stacks'' [http://wiki.ros.org/turtlebot turtlebot] y [http://wiki.ros.org/turtlebot_simulator turtlebot_simulator], que contienen el modelo y los ''plugins'' para [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], respectivamente. Eliminando lo que no necesitamos del modelo del [http://www.turtlebot.com/ turtlebot] y modificacndo el elemento de la base, el contenido del archivo que guardaremos con el nombre "roomba.xacro" en el directorio "urdf" de nuestro ''package'', es el siguiente:

<syntaxhighlight lang="xml">

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

<include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>

<xacro:macro name="roomba">

<material name="Green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>

<link name="base_footprint">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="Green" />
</visual>

<collision>
<origin xyz="0 0 0.017" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>

<link name="base_link">
<inertial>
<mass value="3.6" />
<origin xyz="0 0 ${0.063/2}" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
</inertial>

<visual>
<origin xyz=" 0 0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1" />
</material>
</visual>

<collision>
<origin xyz="0.0 0.0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
</collision>
</link>

<link name="wall_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="left_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="right_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<link name="leftfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>

<link name="rightfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>

<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0.017" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>

<joint name="base_wall_sensor_joint" type="fixed">
<origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />
<parent link="base_link"/>
<child link="wall_sensor_link" />
</joint>

<joint name="base_left_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="left_cliff_sensor_link" />
</joint>

<joint name="base_right_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="right_cliff_sensor_link" />
</joint>

<joint name="base_leftfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="leftfront_cliff_sensor_link" />
</joint>

<joint name="base_rightfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="rightfront_cliff_sensor_link" />
</joint>

<link name="left_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>

<joint name="left_wheel_joint" type="continuous">
<origin xyz="0 0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="right_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>

<joint name="right_wheel_joint" type="continuous">
<origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="rear_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.015" />
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.015" />
</geometry>
</collision>
</link>

<joint name="rear_castor_joint" type="fixed">
<origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="rear_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<link name="front_wheel_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>

<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.018" />
</geometry>
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.018" />
</geometry>
</collision>
</link>

<joint name="front_castor_joint" type="fixed">
<origin xyz="0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="front_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>

<joint name="gyro_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0 0 0.04" rpy="0 0 0" />
<parent link="base_link"/>
<child link="gyro_link"/>
</joint>
<link name="gyro_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
</inertial>
</link>

<joint name="laser_joint" type="fixed">
<origin xyz="-0.065 0 0.57" rpy="0 0 0" />
<parent link="base_link" />
<child link="laser" />
</joint>

<link name="laser" />

<gaezebo>
<material>Gazebo/White</material>
</gaezebo>

<myrabot_sim_imu/>
<myrabot_sim_laser/>
<myrabot_sim_roomba/>
<myrabot_sim_wall_sensors/>

</xacro:macro>

</robot>

</syntaxhighlight>

Para su uso en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] debemos añadir los ''plugins'' de los diferentes sensores de la base. Crearemos un archivo llamado "roomba-gazebo.xacro" en el directorio "urdf" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="myrabot_sim_imu">
<gazebo>
<controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
<robotNamespace>/</robotNamespace>
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<bodyName>gyro_link</bodyName>
<topicName>imu/data</topicName>
<gaussianNoise>${0.0017*0.0017}</gaussianNoise>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="imu_position"/>
</controller:gazebo_ros_imu>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_laser">
<gazebo reference="laser">
<sensor:ray name="laser">
<rayCount>180</rayCount>
<rangeCount>180</rangeCount>
<laserCount>1</laserCount>

<origin>0.0 0.0 0.0</origin>
<displayRays>false</displayRays>

<minAngle>-${60/2}</minAngle>
<maxAngle>${60/2}</maxAngle>

<minRange>0.08</minRange>
<maxRange>5.0</maxRange>
<resRange>0.01</resRange>
<updateRate>20</updateRate>
<controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>scan</topicName>
<frameName>laser</frameName>
<interface:laser name="gazebo_ros_laser_iface" />
</controller:gazebo_ros_laser>
</sensor:ray>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_roomba">
<gazebo>
<controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_create.so">
<alwaysOn>true</alwaysOn>
<node_namespace>myrabot_node</node_namespace>
<left_wheel_joint>left_wheel_joint</left_wheel_joint>
<right_wheel_joint>right_wheel_joint</right_wheel_joint>
<front_castor_joint>front_castor_joint</front_castor_joint>
<rear_castor_joint>rear_castor_joint</rear_castor_joint>
<wheel_separation>.230</wheel_separation>
<wheel_diameter>0.070</wheel_diameter>
<base_geom>base_roomba_link</base_geom>
<updateRate>40</updateRate>
<torque>1.0</torque>
</controller:gazebo_ros_roomba>
</gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_wall_sensors">
<gazebo reference="wall_sensor_link">
<sensor:ray name="wall_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.0160</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="left_cliff_sensor_link">
<sensor:ray name="left_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="right_cliff_sensor_link">
<sensor:ray name="right_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="leftfront_cliff_sensor_link">
<sensor:ray name="leftfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="rightfront_cliff_sensor_link">
<sensor:ray name="rightfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>

<minAngle>0</minAngle>
<maxAngle>0</maxAngle>

<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>

<gazebo reference="left_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>

<gazebo reference="right_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>
<gazebo reference="rear_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>

<gazebo reference="front_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>
</xacro:macro>
</robot>

</syntaxhighlight>

==URDF kinect==

Para la cámara [http://www.xbox.com/es-ES/Kinect kinect] incluiremos el modelo y el ''plugin'' para el simulador en un solo archivo. Crearemos un archivo llamado "kinect.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<property name="M_PI" value="3.14159"/>

<xacro:macro name="kinect" params="parent *origin">

<joint name="base_camera_joint" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="camera_link" />
</joint>

<link name="camera_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>

<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</visual>

<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</collision>

</link>

<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0.018 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_depth_frame" />
</joint>

<link name="camera_depth_frame">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>

<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>

<link name="camera_depth_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<joint name="camera_rgb_joint" type="fixed">
<origin xyz="0 -0.005 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_rgb_frame" />
</joint>

<link name="camera_rgb_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<joint name="camera_rgb_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_rgb_frame" />
<child link="camera_rgb_optical_frame" />
</joint>

<link name="camera_rgb_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>

<gazebo reference="camera_link">
<sensor:camera name="camera">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>60</hfov>
<nearClip>0.05</nearClip>
<farClip>5</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/camera/image_raw</imageTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/camera/camera_info</cameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>

</xacro:macro>

</robot>

</syntaxhighlight>

Los archivos empleados, con el modelo de malla 3D ".stl", en la descrición de la cámara se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro ''package'':

* [http://www.fernando.casadogarcia.es/files/kinect.dae kinect.dae]
* [http://www.fernando.casadogarcia.es/images/kinect.jpg kinect.jpg]

==URDF cámara web==

Para la cámara web vamos a emplear el ''plugin'' de la cámara [http://www.xbox.com/es-ES/Kinect kinect], pero sin usar la nube de puntos (''point cloud''). La cámara web empleada es una [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000]. Crearemos un archivo llamado "webcam.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="webcam" params="parent *origin">

<joint name="webcam_1" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="e_webcam_1_link" />
</joint>

<link name="e_webcam_1_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="webcam_2" type="fixed">
<origin xyz="0 ${0.135-0.04-(0.02-0.009)} ${(0.185/2)+0.0045}" rpy="0.82 0 0" />
<parent link="e_webcam_1_link" />
<child link="e_webcam_2_link" />
</joint>

<link name="e_webcam_2_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="webcam_optica" type="fixed">
<origin xyz="0.135 -0.0285 0" rpy="0 0 -1.57" />
<parent link="e_webcam_2_link" />
<child link="e_webcam_optica_link" />
</joint>

<link name="e_webcam_optica_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
</collision>
</link>

<gazebo reference="e_webcam_1_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_2_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_optica_link">
<material>myrabot_fer/White</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="e_webcam_optica_link">
<sensor:camera name="webcam">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>63.09</hfov>
<nearClip>0.02</nearClip>
<farClip>3</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="webcam_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/webcam/image_raw</imageTopicName>
<pointCloudTopicName>/webcam/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/webcam/camera_info</cameraInfoTopicName>
<frameName>e_webcam_optica_link</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>

</xacro:macro>

</robot>

</syntaxhighlight>

Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la cámara web empleada se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:

* [http://www.fernando.casadogarcia.es/files/webcam_1.stl webcam_1.stl]
* [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]

==URDF sensor ultrasonidos==

===Creación de un plugin para gazebo===

Primero [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] que va a contener los ''plugins'' de nuestro robot, que llamaremos "myrabot_gazebo_plugins", con las dependencias gazebo sensor_msgs roscpp.. Dentro de la carpeta "src" del ''package'' creado para los ''plugins'' crearemos un archivo llamado "gazebo_ros_sonar.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <gazebo/Sensor.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <limits>
#include <myrabot_gazebo_plugin/gazebo_ros_sonar.h>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)

GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
{
sensor_ = dynamic_cast<RaySensor*>(parent);
if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");

Param::Begin(&parameters);
topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
frame_id_param_ = new ParamT<std::string>("frameId", "", false);
radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
fov_param_ = new ParamT<double>("fov", 0.05, false);
gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
Param::End();
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosSonar::~GazeboRosSonar()
{
delete topic_param_;
delete frame_id_param_;
delete radiation_param_;
delete fov_param_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::LoadChild(XMLConfigNode *node)
{
ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
topic_param_->Load(node);
frame_id_param_->Load(node);
radiation_param_->Load(node);
fov_param_->Load(node);
gaussian_noise_->Load(node);
}

///////////////////////////////////////////////////////
// Initialize the controller
void GazeboRosSonar::InitChild()
{
Range.header.frame_id = **frame_id_param_;

if (**radiation_param_==std::string("ultrasound"))
Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
else
Range.radiation_type = sensor_msgs::Range::INFRARED;

Range.field_of_view = **fov_param_;
Range.max_range = sensor_->GetMaxRange();
Range.min_range = sensor_->GetMinRange();

sensor_->SetActive(false);
node_handle_ = new ros::NodeHandle("");
publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
}

////////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double GazeboRosSonar::GaussianKernel(double mu,double sigma)
{
// using Box-Muller transform to generate two independent standard normally disbributed normal variables
// see wikipedia
double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
//double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
// we'll just use X
// scale to our mu and sigma
X = sigma * X + mu;
return X;
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosSonar::UpdateChild()
{
if (!sensor_->IsActive()) sensor_->SetActive(true);

Range.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();

for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
double ray = sensor_->GetRange(i);
if (ray < Range.range) Range.range = ray;
}

Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
publisher_.publish(Range);
}

////////////////////////////////////////////////////////////////////////////////
// Finalize the controller
void GazeboRosSonar::FiniChild()
{
sensor_->SetActive(false);
node_handle_->shutdown();
delete node_handle_;
}

</syntaxhighlight>

Crearemos un archivo llamado "gazebo_ros_sonar.h" situdo en la ruta "include/myrabot_gazebo_plugins/" dentro del ''package'' que hemos creado, con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>

#include <gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/sensors/RaySensor.hh>

#include <ros/callback_queue.h>
#include <sensor_msgs/Range.h>

namespace gazebo
{

class GazeboRosSonar : public gazebo::SensorPlugin
{
public:
GazeboRosSonar();
virtual ~GazeboRosSonar();

protected:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
virtual void Update();

/// \brief Gaussian noise generator
private:
double GaussianKernel(double mu,double sigma);

private:
gazebo::sensors::RaySensorPtr sensor_;

ros::NodeHandle* node_handle_;
ros::Publisher publisher_;

sensor_msgs::Range range_;

std::string namespace_;
std::string topic_name_;
std::string frame_id_;
std::string radiation_;
double fov_;
double gaussian_noise_;

gazebo::physics::WorldPtr parent_;

common::Time last_time;

gazebo::event::ConnectionPtr updateConnection;

};

} // namespace gazebo

</syntaxhighlight>

Añadiremos al archivo "CMakeLists.txt" del ''package'' la siguiente línea al final:

<syntaxhighlight>rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)</syntaxhighlight>

También añadiremos las siguientes líneas al archivo "manifest.xml" del ''package'', antes de las etiqueta </package>:

<syntaxhighlight lang="xml">

<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
<gazebo plugin_path="${prefix}/lib" />
</export>

</syntaxhighlight>

Para compilar el nuevo ''plugin'' simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:

<syntaxhighlight>

roscd myrabot_gazebo_plugins
make

</syntaxhighlight>

===URDF===

En el directorio "urdf" del ''package'' del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">

<joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
<parent link="${parent}"/>
<child link="ultrasonidos_${id}_link_fisico"/>
<insert_block name="origin" />
</joint>

<link name="ultrasonidos_${id}_link_fisico">
<inertial>
<mass value="0.001"/>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
</collision>
</link>

<joint name="ultrasonidos_${id}_joint" type="fixed">
<parent link="ultrasonidos_${id}_link_fisico"/>
<child link="ultrasonidos_${id}_link"/>
<origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/>
</joint>

<link name="ultrasonidos_${id}_link">

</link>

<gazebo reference="ultrasonidos_${id}_link_fisico">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="ultrasonidos_${id}_link">
<sensor:ray name="ultrasonidos_${id}">

<displayRays>false</displayRays>

<rayCount>5</rayCount>
<rangeCount>5</rangeCount>
<verticalRayCount>1</verticalRayCount>
<verticalRangeCount>1</verticalRangeCount>

<minAngle>-16</minAngle>
<verticalMinAngle>-16</verticalMinAngle>
<maxAngle>16</maxAngle>
<verticalMaxAngle>16</verticalMaxAngle>

<minRange>0</minRange>
<maxRange>7</maxRange>
<resRange>0.01</resRange>

<updateRate>20</updateRate>

<controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>ultrasonidos_${id}</topicName>
<frameId>ultrasonidos_${id}_link</frameId>
<radiation>ultrasound</radiation>
<fov>32</fov>

</controller:gazebo_ros_sonar>
</sensor:ray>
</gazebo>
</xacro:macro>

</robot>

</syntaxhighlight>

=Ensamblado del modelo del MYRAbot=

Para poder formar el modelo de nuestro robot tenemos que ensamblar las diferentes partes que lo componen, las que hemos descrito en los puntos anteriores y el [[Control brazo MYRAbot (bioloid+arduino)|brazo]]. Los componentes individuales se han definido como ''macros'' con los parámetros de ''link parent'' y origen, con lo que deberemos indicar el ''link'' y el origen del componente al que se une. Crearemos un archivo llamado "myrabot.urdf.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="MYRAbot"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">

<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />

<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />

<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />

<roomba />

<estructura_myrabot parent="base_link">
<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
</estructura_myrabot>

<kinect parent="e_base_kinect_link">
<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
</kinect>

<brazo parent="e_base_brazo_1_link">
<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
</brazo>

<webcam parent="e_monitor_link">
<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
</webcam>

<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 1.57" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.93" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 4.71" xyz="0.135 0.135 0.02"/>
</ultrasonidos>

</robot>

</syntaxhighlight>

==Visualización del modelo en rviz==

Para la comprobación y visualización del correcto ensamblado del modelo, así como la prueba de las uniones móviles, se ha creado un ''launcher'' para cargar el modelo en [http://wiki.ros.org/rviz rviz]. Crearemos un archivo llamado "myrabot_rviz.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

<param name="use_gui" value="true"/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

<node name="rviz" pkg="rviz" type="rviz" />

</launch>

</syntaxhighlight>

Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>

Si todo va bien, se ejecutará [http://wiki.ros.org/rviz rviz] y el interfaz del [http://wiki.ros.org/joint_state_publisher joint_state_publisher], sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en [http://wiki.ros.org/rviz rviz]. Para que se muestre correctamente nuestro modelo en [http://wiki.ros.org/rviz rviz] debemos seleccionar el elemento ''link'' que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el ''link'' "/base_footprint". El resultado es el que se muestra a continuación:

[[file:modelo_MYRAbot_rviz.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]

=Carga del modelo en gazebo=

==Uso de un único ''topic'' para joint states==

Para poder tener en el ''topic'' "joint_states_myrabot" el estado de las uniones del brazo y de la base, hemos creado un pequeño programa que toma los valores de los ''topics'' "brazo/joint_states" y "joint_states", y los publica en este único ''topic''. Crearemos un archivo llamado "remap_joint_states.cpp" dentro del directorio "src" de nuestro ''package'':

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "ros/time.h"
#include "sensor_msgs/JointState.h"

sensor_msgs::JointState joint_states_todo;
int cont_brazo = 0;
int cont_roomba = 0;
std::string name[9];
float position[9];
float velocity[9];
float effort[9];

void brazo(const sensor_msgs::JointState& brazo_states)
{
int tam = brazo_states.name.size();

if (::cont_brazo == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i] = brazo_states.name[i];
::position[i] = brazo_states.position[i];
::velocity[i] = brazo_states.velocity[i];
::effort[i] = brazo_states.effort[i];
}

::cont_brazo = 1;
}
}

void roomba(const sensor_msgs::JointState& roomba_states)
{
int tam = roomba_states.name.size();

if (::cont_roomba == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i+5] = roomba_states.name[i];
::position[i+5] = roomba_states.position[i];
::velocity[i+5] = roomba_states.velocity[i];
::effort[i+5] = roomba_states.effort[i];
}

::cont_roomba = 1;
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "remap_joint_states");

ros::NodeHandle n;

ros::Subscriber joint_states_brazo_sub_= n.subscribe("brazo/joint_states", 1, brazo);
ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states", 1, roomba);

ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states_myrabot", 1);

ros::Rate loop_rate(100);

ros::Time ahora;

::joint_states_todo.name = std::vector<std::string>(9);
::joint_states_todo.position = std::vector<double>(9);
::joint_states_todo.velocity = std::vector<double>(9);
::joint_states_todo.effort = std::vector<double>(9);

while (ros::ok())
{

if (::cont_brazo == 1 && ::cont_roomba == 1)
{
for (int i = 0; i < 9; i++)
{
::joint_states_todo.name[i] = ::name[i];
::joint_states_todo.position[i] = ::position[i];
::joint_states_todo.velocity[i] = ::velocity[i];
::joint_states_todo.effort[i] = ::effort[i];
}

ahora = ros::Time::now();

::joint_states_todo.header.stamp = ahora;

joint_states_pub_.publish(::joint_states_todo);

::cont_brazo = 0;
::cont_roomba = 0;
}

ros::spinOnce();

loop_rate.sleep();
}

ros::spin();

return 0;
}

</syntaxhighlight>

==Carga en gazebo==

Para poder cargar el modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] e iniciar los controladores de todos sus componentes, se ha creado un ''launcher''. Crearemos un archivo llamado "myrabot_gazebo.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
<node name="gazebo_gui" pkg="gazebo" type="gui" />

<group ns="brazo">

<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />

<node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />

<rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>

<node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />

</group>

<node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />

<node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="false"/>
<param name="vo_used" value="false"/>
<param name="output_frame" value="odom"/>
</node>

<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
<param name="max_rate" value="20.0"/>
<remap from="cloud_in" to="/camera/depth/points"/>
<remap from="cloud_out" to="cloud_throttled"/>
</node>

<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.15"/>
<param name="max_height" value="0.15"/>
<remap from="cloud" to="/cloud_throttled"/>
</node>

<node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>

<param name="min_height" value="-0.025"/>
<param name="max_height" value="0.025"/>
<remap from="cloud" to="/cloud_throttled"/>
<remap from="scan" to="/narrow_scan"/>
</node>

</launch>

</syntaxhighlight>

Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>

Si todo va bien podremos ver en la ventana gráfica de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] el modelo del MYRAbot situado en un mundo vacío, tal como se muestra en la siguiente imagen:

[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

==Programa de prueba (Vuela al ruedo)==

Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a [[Fernando-TFM-ROS02#Creando un package| crear un package]] que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.

Dentro de este ''package'' vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:

<syntaxhighlight lang="cpp" anclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"

int cont = 0;
int cont1= 0;
int encendido = 0;
brazo_fer::Servos move, on, vel;

void home()
{
if (cont1 == 0)
{
ros::NodeHandle n;

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

brazo_fer::WriteServos home;

::move.base = 511;
::move.arti1 = 511;
::move.arti2 = 511;
::move.arti3 = 511;
::move.pinza = 511;

::on.base = 1;
::on.arti1 = 1;
::on.arti2 = 1;
::on.arti3 = 1;
::on.pinza = 1;

::vel.base = 400;
::vel.arti1 = 400;
::vel.arti2 = 400;
::vel.arti3 = 400;
::vel.pinza = 400;

home.posicion = ::move;
home.par = ::on;
home.velocidad = ::vel;

move_arm_pub_.publish(home);
hand_pub_.publish(home);

::cont1 = 1;

}
}

void inicio(const std_msgs::Bool& inicio)
{
if (inicio.data == true && ::cont == 0)
{
::encendido = 1;
::cont = 1;
}
else if (inicio.data == true && ::cont == 1)
{
::encendido = 0;
::cont = 0;
}
}

void posicion(const brazo_fer::ReadServos& posicion)
{
ros::NodeHandle n;

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

brazo_fer::Servos p = posicion.posicion;
brazo_fer::WriteServos mover_brazo;

home();

if (::encendido == 1)
{
geometry_msgs::Twist mover_base;

mover_base.linear.x = 0.1;
mover_base.angular.z = 0.3;

move_base_pub_.publish(mover_base);

if (::cont1 == 1)
{
::move.base = 718;
::cont1 = 2;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
{
if (::cont1 == 2)
{
::move.base = 304;
::cont1 = 3;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else
{
::cont1 = 1;
}
}
}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "vuelta_al_ruedo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio);

ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':

<syntaxhighlight>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>
roscd myrabot_fer
make
</syntaxhighlight>

Para lanzar [http://wiki.ros.org/simulator_gazebo gazebo] con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal:

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>

Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>

El modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el ''topic'' "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal:

<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</syntaxhighlight>

Si todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:

<center><videoflash>IY63zJgQYGI</videoflash></center>

<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Ver vídeo en YouTube]</center>

=Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja=

Para la ejecución del programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]] en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.

==Creación de modelos de objetos==

===Mesa===

Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).

[[file:modelo_mesa.png|thumb|200px|center|Modelo mesa en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

Para contener los objetos vamos a [[Fernando-TFM-ROS02#Creando un package|crear un ''package'']] que llamaremos "objetos_fer_modelos" con las siguientes dependencias: geometry_msgs gazebo urdf roscpp. Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del ''package'' que acabamos de crear, con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="mesa" type="floating">
<parent link="world"/>
<child link="mesa_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="mesa_link">
<visual>
<geometry>
<box size="2 0.75 0.03" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<geometry>
<box size="2 0.75 0.03" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<mass value="5" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>

<xacro:macro name="pata" params="id refx refy">
<joint name="pata_${id}" type="fixed">
<parent link="mesa_link"/>
<child link="pata_${id}_link"/>
<origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
</joint>

<link name="pata_${id}_link">
<visual>
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<mass value="3" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
</xacro:macro>

<xacro:pata id="1" refx="1" refy="1"/>
<xacro:pata id="2" refx="1" refy="-1"/>
<xacro:pata id="3" refx="-1" refy="-1"/>
<xacro:pata id="4" refx="-1" refy="1"/>

<gazebo reference="mesa_link">
<material>Gazebo/LightWood</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<xacro:macro name="gazebo_pata" params="id">
<gazebo reference="pata_${id}_link">
<material>PR2/Black</material>
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</xacro:macro>

<xacro:gazebo_pata id="1"/>
<xacro:gazebo_pata id="2"/>
<xacro:gazebo_pata id="3"/>
<xacro:gazebo_pata id="4"/>

</robot>

</syntaxhighlight>

Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.

===Creación de nuevos materiales para gazebo===

Comenzaremos creando dentro de nuestro ''package'' un directorio llamado "Media", dentro del cual crearemos otro directorio llamado "materials", y dentro de este crearemos otros dos directorios llamados "scripts" y "textures". También debemos añadir las siguientes líneas al archivo "manifest.xml" de nuestro ''package'', para colocarlo en la ruta de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

<syntaxhighlight lang="xml">

<export>
<gazebo gazebo_media_path="${prefix}" />
</export>

</syntaxhighlight>

Los materiales se definen para el motor gráfico [http://www.ogre3d.org/ OGRE]. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" creada dentro de nuestro ''package''. Crearemos un archivo llamado "myrabot_fer.material" dentro del directorio "scripts" de nuestro ''package'' con el contenido que se muestra a continuación, para la definición de un nuevo material con textura:

<syntaxhighlight>

material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture NUESTRO_ARCHIVO_DE_IMAGEN
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

===Latas===

====Lata de cerveza====

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>

<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<gazebo reference="lata_link">
<material>material_fer/Amstel</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

</robot>

</syntaxhighlight>

Ahora debemos añadir el material "Amstel" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":

<syntaxhighlight>

material material_fer/Amstel
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture amstel.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

[[file:amstel.png|thumb|200px|center|Textura lata de cerveza]]

[[file:modelo_lata_cerveza.png|thumb|200px|center|Lata de cerveza en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

====Lata de cola====

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>

<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>

<gazebo reference="lata_link">
<material>material_fer/CocaCola</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>

</robot>

</syntaxhighlight>

Ahora debemos añadir el material "CocaCola" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":

<syntaxhighlight>

material Gazebo/CocaCola
{
technique
{
pass
{
lighting off

ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0

alpha_rejection greater 128
depth_write on

texture_unit
{
texture coca_cola.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}

</syntaxhighlight>

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:

[[file:coca_cola.png|thumb|200px|center|Textura lata de cola]]

[[file:modelo_lata_cola.png|thumb|200px|center|Lata de cola [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

Para poder cargar los modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] debemos convertirlos a ".urdf", para extender las macros y calcular campos, de nuestros archivos ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>
roscd objetos_fer_modelos
cd urdf
rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdf
</syntaxhighlight>

==Ejecución==

Para la carga de todos los modelos se ha creado un ''launcher'', donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del ''package'' "myrabot_fer", con el sigiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>

<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />

<node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />

<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />

<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

</syntaxhighlight>

Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:

<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]], en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:

<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>

Para seleccionar el objeto a señalar publicaremos en el ''topic'' "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>

Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:

<center><videoflash>lp2FUFlsFV4</videoflash></center>

<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Ver vídeo en YouTube]</center>

=Ejecución del programa seguir objetos=

==Programa para mover objetos en gazebo usando el teclado==

Para poder desplazar los modelos de los objetos dentro de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] hemos escrito un pequeño programa que publica en el ''topic'' "gazebo/set_model_state" la posición del modelo del objeto indicado, en este caso "Lata_amstel", usando el teclado. Crearemos un archivo llamado "mover_objetos.cpp" que guardaremos en el directorio "src" del ''package'' que hemos creado anteriormente para contener los modelos de los objetos, con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "gazebo/ModelState.h"
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

#define KEYCODE_Xmas 0x43
#define KEYCODE_Xmenos 0x44
#define KEYCODE_Ymas 0x41
#define KEYCODE_Ymenos 0x42
#define KEYCODE_Zmas 0x77
#define KEYCODE_Zmenos 0x73
#define KEYCODE_XgiroMas 0x61
#define KEYCODE_XgiroMenos 0x64
#define KEYCODE_YgiroMas 0x65
#define KEYCODE_YgiroMenos 0x71

struct termios cooked, raw;

int cont = 0;

double retardo = 0.1;

gazebo::ModelState objeto;

void quit(int sig)
{
tcsetattr(0, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}

void callback(const ros::TimerEvent&)
{

ros::NodeHandle n;

ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

std::string modelo_objeto;

ros::NodeHandle nh("~");

nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));

ros::Time anterior;

if (::cont == 0)
{
anterior = ros::Time::now();

::objeto.model_name = modelo_objeto;

::objeto.reference_frame = "world";

::objeto.pose.position.x = 0;

::objeto.pose.position.y = 0.5;

::objeto.pose.position.z = 0.74;

::objeto.pose.orientation.x = 0;

::objeto.pose.orientation.y = 0;

::objeto.pose.orientation.z = 0;

::objeto.pose.orientation.w = 1;

::cont = 1;

object_pub_.publish(::objeto);

}

char c;

// get the console in raw mode
tcgetattr(0, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(0, TCSANOW, &raw);

puts("");
puts("#####################################################");
puts(" CONTROLES OBJETO");
puts("#####################################################");
puts("");
puts("Flecha arriba:_______ Subir objeto");
puts("Flecha abajo:________ Bajar objeto");
puts("Flecha izquierda:____ Desplazar objeto a la izquierda");
puts("Flecha derecha:______ Desplazar objeto a la derecha");
puts("Tecla W:_____________ Desplazar objeto hacia delante");
puts("Tecla S:_____________ Desplazar objeto hacia atrás");
puts("Tecla A:_____________ Inclinar objeto en eje x mas");
puts("Tecla D:_____________ Inclinar objeto en eje x menos");
puts("Tecla Q:_____________ Inclinar objeto en eje y mas");
puts("Tecla E:_____________ Inclinar objeto en eje y menos");

while (ros::ok())
{

// get the next event from the keyboard
if(read(0, &c, 1) < 0)
{
perror("read():");
exit(-1);
}

if (c == KEYCODE_Xmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Xmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;

anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;

anterior = ros::Time::now();
}
}

object_pub_.publish(::objeto);

}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "mover_objetos");

ros::NodeHandle n;

ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

signal(SIGINT,quit);

ros::Timer timer = n.createTimer(ros::Duration(1), callback);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':

<syntaxhighlight>rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)</syntaxhighlight>

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>
roscd objetos_fer_modelos
make
</syntaxhighlight>

==Ejecución==

Para la carga de todos los modelos se ha creado un ''launcher'', donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_lata.launch" dentro del directorio "launch" del ''package'' "myrabot_fer", con el sigiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<launch>

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>

<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />

<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />

<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

</syntaxhighlight>

Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:

<syntaxhighlight>roslaunch myrabot_fer myrabot_lata.launch</syntaxhighlight>

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa [[Detección y cálculo de posición de objetos (cámara web)#Seguir objetos|seguir objetos]], en el que el brazo sigue el objeto que le indiquemos. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:

<syntaxhighlight>roslaunch brazo_fer seguir.launch</syntaxhighlight>

Para seleccionar el objeto a señalar publicaremos en el ''topic'' "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>

Para poder mover la lata, ejecutaremos en un nuevo terminal el siguiente comando, donde indicamos en el parámetro "modelo_objeto" el nombre del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] que queremos mover:

<syntaxhighlight>rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel</syntaxhighlight>

Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:

<center><videoflash>kLfgfHo8OnI</videoflash></center>

<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

[[Mobile manipulation | < volver a principal]]

Modelo para simulación brazo MYRAbot (urdf+gazebo)

2014-09-29T11:03:17Z

Fernando:

Detección y cálculo de posición de objetos (cámara web)

2014-09-29T11:02:32Z

Fernando:

Control brazo MYRAbot (bioloid+arduino)

2014-09-29T11:01:46Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Brazo Bioloid=

El brazo en el que se basa este proyecto es el desarrollado en el proyecto [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]], realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:
[[file:IMG_4871_b.JPG|thumb|200px|Fotografía del Brazo [http://www.robotis.com/xe/BIOLOID_main_en Bioloid].]]

* Placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560].
* Circuito integrado [http://www.futurlec.com/74LS/74LS241.shtml 74LS241] (Bufer tri-estado).
* Circuito regulador de tensión.
* 5 servomotores serie [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A].
* Piezas de montaje de kit Bioloid.
* Piezas realizadas para pinza y fijación al MYRAbot.

=Arduino IDE y rosserial=

Para la comunicación e intercambio de información entre [http://www.arduino.cc arduino] y [http://www.ros.org ROS] es necesario instalar [http://www.arduino.cc arduino] IDE y rosserial (''stack'' de [http://www.ros.org ROS] que contiene el ''package'' rosserial_arduino con las librerias para [http://www.arduino.cc arduino]). Comenzaremos instalando el [http://www.arduino.cc arduino] IDE, para lo que ejecutaremos los siguientes comandos en un terminal:

<syntaxhighlight>sudo apt-get update
sudo apt-get install arduino arduino-core</syntaxhighlight>

Una vez realizada la instalación del software de [http://www.arduino.cc arduino] se procedera a la instalación del ''package'' de [http://www.ros.org ROS] ejecutando en un terminal el siguiente comando:

<syntaxhighlight>sudo apt-get install ros-NUESTRA_VERSIÓN_ROS-rosserial</syntaxhighlight>

Instalados [http://www.arduino.cc arduino] IDE y el ''package'' rosserial debemos copiar las librerias del ''package'' rosserial_arduino al sketchbook de [http://www.arduino.cc arduino], carpeta que se encuentra habitualmente en la carpeta personal, para lo que ejecutaremos en un terminal los siguientes comandos:

<syntaxhighlight>roscd rosserial_arduino/libraries
cp -r ros_lib /home/”nombre_sesion”/sketchbook/libraries/ros_lib</syntaxhighlight>

==Creación de un package para nuestros programas==

Deberemos tener previamente creado un [[Fernando-TFM-ROS02#Creando un espacio de trabajo|espacio de trabajo]] para nuestra versión de [http://www.ros.org ROS]. Para poder compilar y enviar nuestros programas a la placa [http://www.arduino.cc arduino] sin tener que pasar por [http://www.arduino.cc arduino] IDE vamos a crear un ''package'' llamado “arduino_fer” con las dependencias necesarias para nuestros programas, para ello ejecutaremos los siguientes comandos en un terminal:

<syntaxhighlight>cd ~/ros_workspace
roscreate-pkg arduino_fer rosserial_arduino std_msgs</syntaxhighlight>

Deberemos sustituir el contenido del fichero "CMakeLists.txt", situado en la carpeta del ''package'' creado, por el siguiente:

<syntaxhighlight lang=cmake>cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_find_ros_package(rosserial_arduino)
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)</syntaxhighlight>

Para finalizar la cración del ''package'' ejecutaremos los siguientes comandos:

<syntaxhighlight>roscd arduino_fer
cmake .</syntaxhighlight>

==Primer programa (A toda potencia)==

El primer programa que vamos a realizar crea un nodo llamado “potencia” que publica un ''topic'' llamado “cifra” y está suscrito a un ''topic'' llamado “resultado”. Cuando se publique un número en el ''topic'' “cifra” lo multiplicará por si mismo y publicará el resultado en el ''topic'' “resultado”. El código del programa es el siguiente:
<syntaxhighlight lang=cpp>
#include <ros.h>
#include <std_msgs/Int16.h>

ros::NodeHandle nh;
int pot;

void potencia( const std_msgs::Int16& cifra){

::pot = cifra.data*cifra.data;
}

ros::Subscriber<std_msgs::Int16> sub("cifra", &potencia );
std_msgs::Int16 res;

ros::Publisher pub("resultado", &res);

void setup()
{
nh.initNode();
nh.subscribe(sub);
nh.advertise(pub);
}

void loop()
{
res.data = ::pot;
pub.publish( &res );
nh.spinOnce();
delay(1000);
}
</syntaxhighlight>
Para su compilación y envio a la placa [http://www.arduino.cc arduino] es necesario añadir al archivo "CMakeLists.txt" las siguientes líneas:

<syntaxhighlight lang=cmake>set(FIRMWARE_NAME potencia)

set(${FIRMWARE_NAME}_BOARD MODELO_NUESTRA_PLACA) # Modelo placa arduino
set(${FIRMWARE_NAME}_SRCS src/potencia.cpp )
set(${FIRMWARE_NAME}_PORT /dev/ttyACM0) # Puerto serie para carga
generate_ros_firmware(${FIRMWARE_NAME})</syntaxhighlight>

Donde se indica el nombre del programa, tipo de placa [http://www.arduino.cc arduino] (uno, atmega328 o mega2560), nombre y ubicación del archivo y puerto serie del PC empleado para la comunicación con la placa. Deberemos ejecutar los siguientes comandos en un terminal:

<syntaxhighlight>roscd arduino_fer
make potencia-upload</syntaxhighlight>

Para la ejecución del programa se debe lanzar en un terminal el núcleo de [http://www.ros.org ROS], si no se encuentra ya en marcha, ejecutando el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

En otro terminal se ejecutará el nodo que comunica a [http://www.ros.org ROS] con la placa [http://www.arduino.cc arduino], indicándole el puerto empleado para la comunicación. Para saber el puerto que se está empleando, con la placa [http://www.arduino.cc arduino] conectada al pc a través de su puerto USB, podemos verlo en el menú "Herramientas">"Puerto serie" del software [http://www.arduino.cc arduino] IDE. En nuestro caso es el "ttyACM0":

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para la publicación de un número en el ''topic'' “cifra” se ejecutará en otro terminal el siguiente comando:

<syntaxhighlight>rostopic pub cifra std_msgs/Int16 NUMERO_DESEADO --once</syntaxhighlight>

Para comprobar que realmente el programa calcula el cuadrado del número publicado en el ''topic'' “cifra” podemos ejecutar en otro terminal el siguiente comando para visualizar el ''topic'' “resultado”:

<syntaxhighlight>rostopic echo resultado</syntaxhighlight>

=Arduino y servomotores Dinamixel=

En [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] se encuentran las adaptaciones hardware realizadas a la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] para la correcta comunicación con los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] de [http://www.robotis.com/xe/ ROBOTICS], así como las librerías utilizadas para la programación ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).

==Segundo programa (Muévete)==

En este programa simplemente se realiza un prueba de la comunicación entre la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] con un servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. El programa publica el ''topic'' “angulo” con la posición del motor (entero entre 0 y 1023) y está suscrito al ''topic'' “giro” del que recibe la posición a la que debe moverse (entero entre 0 y 1023), con una velocidad constante de 128 (entero entre 0 y 1023). El código del programa es el siguiente:

<syntaxhighlight lang=cpp>
#include <ros.h>
#include <std_msgs/Int16.h>
#include <DynamixelSerial1.h>

ros::NodeHandle nh;

void mover( const std_msgs::Int16& giro){

Dynamixel.moveSpeed(1,giro.data,128);

}

ros::Subscriber<std_msgs::Int16> sub("giro", &mover );

std_msgs::Int16 ang;
ros::Publisher pub("angulo", &ang);

void setup()
{
nh.initNode();
nh.subscribe(sub);
nh.advertise(pub);
Dynamixel.begin(1000000,2);
delay(1000);
}

void loop()
{
int posicion = Dynamixel.readPosition(1);
ang.data = posicion;
pub.publish( &ang );
nh.spinOnce();
delay(10);
}
</syntaxhighlight>
Para poder compilar y enviar el programa a la placa [http://www.arduino.cc arduino] deberemos emplear el software [http://www.arduino.cc arduino] IDE, ya que empleamos una librería externa al sistema [http://www.ros.org ROS] y no nos permite hacerlo usando [[#Creación de un package para nuestros programas|nuestro ''package'']].
Una vez transferido el programa a la placa, para probar el programa debemos ejecutar en un terminal el siguiente comando, que inicia el nucleo de [http://www.ros.org ROS]:

<syntaxhighlight>roscore</syntaxhighlight>

También debemos lanzar en otro terminal el nodo de comunicación [http://www.ros.org ROS]-[http://www.arduino.cc arduino], ejecutando el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

En un nuevo terminal publicaremos en el ''topic'' "giro" la posición de destino del servomotor, ejecutando el siguiente comando:

<syntaxhighlight>rostopic pub giro std_msgs/Int16 POSICION_DESEADA --once</syntaxhighlight>

El servomotor de inmediato comenzará a moverse hasta la posición indicada. Podemos visualizar el valor de posición del servomotor en todo momento ejecutando en otro terminal el siguiente comando:

<syntaxhighlight>rostopic echo angulo</syntaxhighlight>

=Modelo cinemático del brazo=

Para el correcto posicionamiento del brazo es necesario obtener las transformaciones geométricas para determinar el ángulo que deben girar los servomotores para alcanzar un punto (cinemática inversa). Al tratarse de un brazo que trabaja en un solo plano con giro (4 articulaciones) es un proceso sencillo que se ha realizado de forma directa, sino se puede recurrir al algoritmo de [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters parametrización Denavit- Hartenberg] que permite obtener la cinemática directa y a partir de esta obtener la inversa.

[[file:brazo_cinematica.jpg|thumb|500px|center|Diagramas para el estudio de la cinemática]]

==Cinemática inversa==

Siguiendo los diagramas planteados y considerando ''x'',''y'' y ''z'' como las coordenadas del punto destino, y ε el ángulo que forma la pinza respecto al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:

[[file:brazo_ecuaciones.jpg]]

Teniendo en cuenta los ángulos límite de los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] y el rango de valores que debemos usar, los valores que debemos pasar a los servomotores de cada articulación son los siguientes:

[[file:brazo_dynamixel_AX-12.jpg|thumb|250px|Diagrama de ángulos servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]]]

[[file:brazo_ecuacion_base.jpg]] Se suman 150º al ángulo para situar el 0º coincidente con el eje z.

[[file:brazo_ecuacion_arti1.jpg]] Se suman 60º al ángulo para situar el 0º coincidente con el eje z.

[[file:brazo_ecuacion_arti2.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.

[[file:brazo_ecuacion_arti3.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.

Para enviar a los servomotores valores dentro del rango de trabajo, se multiplica por 1023 (0x3FF), valor máximo del rango de posiciones del servo, y se divide por 300 (300º), el ángulo total de giro del servomotor.

==Cinemática directa==

A partir de las ecuaciones anteriores podemos plantear también el proceso inverso, la obtención del punto en que se encuentra el brazo a partir de las posiciones de los servomotores. Las ecuaciones obtenidas son las siguientes:

[[file:brazo_ecuaciones_directa.jpg]]

==Espacio de trabajo y limitaciones==

Para evitar colisiones del brazo con las diferentes partes del MYRAbot y con sus propios componentes, es necesario fijar unos límites de giro a los servomotores y restringir el acceso a regiones del espacio. Para esta tarea se ha realizado una hoja de calculo con las formulas de la cinemática inversa y se ha representado en unos gráficos las posiciones de los ejes y las articulaciones del brazo en el espacio.

[[file:brazo_cinematica_excel.jpg|thumb|750px|center|Hoja de cálculo cinemática inversa y directa, [http://www.fernando.casadogarcia.es/files/cinematica_brazo_MYRAbot.xlsx descargar]]]

=Programas de control=

==Tercer programa (Ven aquí)==

Primero crearemos un programa que actuará de intermediario entre el programa de control y los servomotores del brazo, que situaremos en la placa [http://www.arduino.cc arduino]. Este programa se encargará de enviar las ordenes recibidas a los servomotores y enviar los datos más relevantes de los servomotores. Como se van a ''publicar'' y ''suscribir'' los datos de 5 servomotores, vamos a comenzar creando nuestros ''mensajes personalizados''.

===Creación de mensajes personalizados en ROS===

Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas (std_msgs geometry_msgs roscpp). Dentro de este ''package'' crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre de campo).

Comenzaremos creando un mensaje base llamado "servos", que contendrá los datos de cada servomotor. Crearemos un archivo llamado "Servos.msg" con el siguiente contenido:

<syntaxhighlight>int16 base
int16 arti1
int16 arti2
int16 arti3
int16 pinza</syntaxhighlight>

Vamos a crear un mensaje que contenga los datos que enviaremos a los servomotores (posición, velocidad y par). Ahora el tipo de dato que vamos a manejar es el que hemos creado anteriormente. Creamos un archivo llamado "WriteServos.msg" con el siguiente contenido:

<syntaxhighlight>Servos posicion
Servos velocidad
Servos par</syntaxhighlight>

Para la recepción de datos de los servomotores (posición, estado y corriente) crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:

<syntaxhighlight>Servos posicion
Servos estado
Servos corriente</syntaxhighlight>

Para permitir la creación de los mensajes en el sistema [http://www.ros.org ROS] debemos editar el archivo "CMakeLists.txt" eliminando el símbolo "#" para que deje de estar comentada la siguiente línea:

<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>

Para la compilación y creación de los mensaje ejecutaremos la siguiente secuencia de comandos en un terminal, donde "brazo_fer" es el package creado para nuestros programas:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

Como vamos a usar estos mensajes en [http://www.arduino.cc arduino], necesitamos añadirlos al ''package'' "rosserial", ejecutando en un terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>

===Programa arduino===

El programa para la placa [http://www.arduino.cc arduino] está suscrito al ''topic'' "move_arm", por el cual recibe los movimientos de los servomotores, y publica el ''topic'' "pose_arm", con la información de posición, par y corriente de los servomotores. El código del programa es el siguiente:

<syntaxhighlight lang="cpp">

#include <ros.h>
#include <brazo_fer/Servos.h>
#include <brazo_fer/WriteServos.h>
#include <brazo_fer/ReadServos.h>
#include <std_msgs/Bool.h>
#include <math.h>

#include <DynamixelSerial1.h>

ros::NodeHandle nh;

void mover(const brazo_fer::WriteServos& brazo){

brazo_fer::Servos par = brazo.par;

brazo_fer::Servos vel = brazo.velocidad;

brazo_fer::Servos move = brazo.posicion;

int posicion[4];

if (par.base == 0){
Dynamixel.torqueStatus(1,OFF);
}
else {
Dynamixel.torqueStatus(1,ON);

Dynamixel.moveSpeed(1,move.base,vel.base);
}

if (par.arti1 == 0){
Dynamixel.torqueStatus(2,OFF);
}
else {
Dynamixel.torqueStatus(2,ON);

Dynamixel.moveSpeed(2,move.arti1,vel.arti1);
}

if (par.arti2 == 0){
Dynamixel.torqueStatus(3,OFF);
}
else {
Dynamixel.torqueStatus(3,ON);

Dynamixel.moveSpeed(3,move.arti2,vel.arti2);
}

if (par.arti3 == 0){
Dynamixel.torqueStatus(4,OFF);
}
else {
Dynamixel.torqueStatus(4,ON);

Dynamixel.moveSpeed(4,move.arti3,vel.arti3);
}

}

void pinza(const brazo_fer::WriteServos& pinza){

brazo_fer::Servos par = pinza.par;

brazo_fer::Servos vel = pinza.velocidad;

brazo_fer::Servos move = pinza.posicion;

int posicion;

if (par.pinza == 0){
Dynamixel.torqueStatus(5,OFF);
}
else {
Dynamixel.torqueStatus(5,ON);

Dynamixel.moveSpeed(5,move.pinza,vel.pinza);
}

}

ros::Subscriber<brazo_fer::WriteServos> move_sub("move_arm", &mover );
ros::Subscriber<brazo_fer::WriteServos> hand_sub("hand_arm", &pinza );

brazo_fer::ReadServos pec;
std_msgs::Bool pulsador;

ros::Publisher pose_pub("pose_arm", &pec);

void setup()
{
nh.initNode();
nh.subscribe(move_sub);
nh.subscribe(hand_sub);
nh.advertise(pose_pub);

Dynamixel.begin(1000000,2);

}

void loop()
{
brazo_fer::Servos pos;
brazo_fer::Servos est;
brazo_fer::Servos cor;

pos.base = Dynamixel.readPosition(1);
pos.arti1 = Dynamixel.readPosition(2);
pos.arti2 = Dynamixel.readPosition(3);
pos.arti3 = Dynamixel.readPosition(4);
pos.pinza = Dynamixel.readPosition(5);

est.base = Dynamixel.moving(1);
est.arti1 = Dynamixel.moving(2);
est.arti2 = Dynamixel.moving(3);
est.arti3 = Dynamixel.moving(4);
est.pinza = Dynamixel.moving(5);

cor.base = Dynamixel.readLoad(1);
cor.arti1 = Dynamixel.readLoad(2);
cor.arti2 = Dynamixel.readLoad(3);
cor.arti3 = Dynamixel.readLoad(4);
cor.pinza = Dynamixel.readLoad(5);

pec.posicion = pos;
pec.estado = est;
pec.corriente = cor;

pose_pub.publish( &pec );

nh.spinOnce();

}

</syntaxhighlight>

Para compilar y cargar el programa en la placa emplearemos el programa [http://www.arduino.cc arduino] IDE. Este programa nos servirá para otros programas de control que desarrollemos, ya que se trata de un mero intermediario.

===Programa ROS===

Usando las ecuaciones de la cinemática inversa este programa indica a los servomotores la posición de giro para alcanzar el punto de destino que se encuentre dentro del espacio de trabajo. Está suscrito al ''topic'' "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al ''topic'' "pose_arm", donde recibe la información de los servomotores que publica el programa de [http://www.arduino.cc arduino], y publica el ''topic'' "move_arm", donde indica al programa de [http://www.arduino.cc arduino] la posición, velocidad y par de los servomotores. Dentro del ''pakage'' creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "geometry_msgs/Point.h"
#include "math.h"

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

brazo_fer::Servos move, vel, p, e, c;

void posicion(const brazo_fer::ReadServos& pose)
{

::p = pose.posicion;
::e = pose.estado;
::c = pose.corriente;

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

double x = point.x;
double y = point.y;
double z = point.z;

y = y + z*tan(atan2(45,250));

int coordenadas_correctas = 1;

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
double z_p;
double L_a, L;

epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal

alfa = (atan2(x,z)*180)/PI;

z_p = sqrt(pow(z,2)+pow(x,2));

L = sqrt(pow(z_p,2)+pow(y,2));

L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));

beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta = ((beta_p+beta_a)*180)/PI;

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

delta_a = PI-(beta_a+gamma);

gamma = (gamma*180)/PI;

delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (beta_pp > beta_p) {
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
}
else {
delta = ((delta_p+delta_a)*180)/PI;

if (isnan(delta)) {
delta = ((PI+delta_a)*180)/PI;
}
}

if (isnan(gamma)) // si no hay solución
{
coordenadas_correctas = 0;
}

::move.base = ((alfa+150)*1023)/300;
::move.arti1 = ((beta+60)*1023)/300;
::move.arti2 = ((gamma-30)*1023)/300;
::move.arti3 = ((delta-30)*1023)/300;
::move.pinza = 511;

::vel.base = abs(::move.base - ::p.base)/5;
::vel.arti1 = abs(::move.arti1 - ::p.arti1)/5;
::vel.arti2 = abs(::move.arti2 - ::p.arti2)/5;
::vel.arti3 = abs(::move.arti3 - ::p.arti3)/5;
::vel.pinza = abs(::move.pinza - ::p.pinza);

if (coordenadas_correctas == 1 && (205 <= ::move.base && ::move.base <= 818) && (120 <= ::move.arti1 && ::move.arti1 <= 920) && ::move.arti2 >= 50 && ::move.arti3 <= 828) {

brazo_fer::WriteServos mover;

mover.posicion = ::move;

mover.velocidad = ::vel;

mover.par.base = 1;
mover.par.arti1 = 1;
mover.par.arti2 = 1;
mover.par.arti3 = 1;
mover.par.pinza = 1;

move_pub_.publish(mover);
}
else {
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
}

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_brazo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);

ros::Subscriber point_sub_= n.subscribe("point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>rosbuild_add_executable(control_v01 src/control_v01.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Ejecutando el programa===

Para ejecutar el programa debemos lanzar el núcleo de [http://www.ros.org ROS], si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

Ahora debemos iniciar el nodo de la placa [http://www.arduino.cc arduino] ejecutando en un nuevo terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para ejecutar el programa de control llamado "control_v01", en un terminal nuevo ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>

Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el ''topic'' "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:

<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>

El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde uno de partida:

<center><videoflash>iwAcutO3C8g</videoflash></center>

<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Ver vídeo en YouTube]</center>

==Cuarto programa (Agarra la botella)==

===Archivo include con funciones===

Con el objetivo de simplificar y hacer más legibles los programas se ha creado este archivo que contiene las funciones de control del brazo. Crearemos un archivo llamado "brazo_fer.h" dentro del directorio "include/brazo_fer" del ''package'' creado con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#ifndef brazo_fer_H
#define brazo_fer_H

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "std_msgs/Int16.h"
#include "geometry_msgs/Point.h"
#include "math.h"

#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60

geometry_msgs::Point directa(brazo_fer::Servos posicion_servos_0, int inclinacion_pinza)
{

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p1, delta_p2, epsilon;
double z_p;
double L_a, L;
double a, b;
double x, y, z;

alfa = (((posicion_servos_0.base*300)/1023)-150)*PI/180;
beta = (((posicion_servos_0.arti1*300)/1023)-60)*PI/180;
gamma = (((posicion_servos_0.arti2*300)/1023)+30)*PI/180;
delta = (((posicion_servos_0.arti3*300)/1023)+30)*PI/180;

epsilon = (inclinacion_pinza*PI)/180;

L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta_p = beta - beta_a;

delta_a = PI - (beta_a + gamma);

delta_p1 = delta - delta_a;

delta_p2 = 2*PI - (delta - delta_a);

if (delta_p1 < delta_p2)
{
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p1));
}
else
{
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p2));
}

z_p = L_a*cos(beta_p) + Lp*cos(epsilon);

beta_pp = acos(z_p/L);

a = L1*sin(beta);

b = L2*sin(beta+gamma)+Lp*sin(epsilon);

if (a >= b)
{
y = L*sin(beta_pp);
}
else
{
y = -L*sin(beta_pp);
}

z = z_p*cos(alfa);

x = z_p*sin(alfa);

geometry_msgs::Point punto;

punto.x = x;
punto.y = y;
punto.z = z;

return punto;
}

brazo_fer::WriteServos inversa(geometry_msgs::Point destino, int inclinacion_pinza, brazo_fer::Servos posicion_servos_0, int velocidad)
{

double x = destino.x;
double y = destino.y;
double z = destino.z;

int coordenadas_correctas = 1;

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
double z_p;
double L_a, L;

epsilon = (inclinacion_pinza*PI)/180;

alfa = (atan2(x,z)*180)/PI;

z_p = sqrt(pow(z,2)+pow(x,2));

L = sqrt(pow(z_p,2)+pow(y,2));

L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));

beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta = ((beta_p+beta_a)*180)/PI;

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

delta_a = PI-(beta_a+gamma);

gamma = (gamma*180)/PI;

delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (beta_pp > beta_p) {
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
}
else {
delta = ((delta_p+delta_a)*180)/PI;

if (isnan(delta)) {
delta = ((PI+delta_a)*180)/PI;
}
}

if (isnan(gamma))
{
coordenadas_correctas = 0;
}

brazo_fer::Servos posicion_servos_1;
brazo_fer::Servos velocidad_servos;
brazo_fer::Servos par_servos;

posicion_servos_1.base = ((alfa+150)*1023)/300;
posicion_servos_1.arti1 = ((beta+60)*1023)/300;
posicion_servos_1.arti2 = ((gamma-30)*1023)/300;
posicion_servos_1.arti3 = ((delta-30)*1023)/300;

if (velocidad == 0)
{
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)/5;
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)/5;
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)/5;
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)/5;
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}

}
else
{
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)*(velocidad/10);
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)*(velocidad/10);
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)*(velocidad/10);
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)*(velocidad/10);
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}

}

brazo_fer::Servos velocidad_servos_0;

velocidad_servos_0.base = 0;
velocidad_servos_0.arti1 = 0;
velocidad_servos_0.arti2 = 0;
velocidad_servos_0.arti3 = 0;

par_servos.base = 1;
par_servos.arti1 = 1;
par_servos.arti2 = 1;
par_servos.arti3 = 1;

brazo_fer::WriteServos move_arm;

if (coordenadas_correctas == 1 && (205 <= posicion_servos_1.base && posicion_servos_1.base <= 818) && (120 <= posicion_servos_1.arti1 && posicion_servos_1.arti1 <= 920) && posicion_servos_1.arti2 >= 50 && (posicion_servos_1.arti3 <= 828 && posicion_servos_1.arti3 >= 195)) {
move_arm.posicion = posicion_servos_1;
move_arm.velocidad = velocidad_servos;
move_arm.par = par_servos;
return move_arm;
}
else {
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
move_arm.posicion = posicion_servos_0;
move_arm.velocidad = velocidad_servos_0;
move_arm.par = par_servos;
return move_arm;
}
}

brazo_fer::WriteServos control_pinza(brazo_fer::Servos posicion_servos_1, brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
{

brazo_fer::Servos velocidad_servos;
brazo_fer::Servos par_servos;

velocidad_servos.pinza = 50;

par_servos.pinza = 1;

brazo_fer::WriteServos hand_arm;

if ((posicion_servos_1.pinza >= 480 && posicion_servos_1.pinza <= 680 && corriente_servos.pinza <= 300) || (posicion_servos_0.pinza > posicion_servos_1.pinza && posicion_servos_1.pinza >= 480))
{
hand_arm.posicion = posicion_servos_1;
hand_arm.velocidad = velocidad_servos;
hand_arm.par = par_servos;
return hand_arm;
}
else
{
std::cout<<"Alcanzado límite de la pinza"<<std::endl;
hand_arm.posicion = posicion_servos_0;
hand_arm.velocidad = velocidad_servos;
hand_arm.par = par_servos;
return hand_arm;
}
}

geometry_msgs::Point home(brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

geometry_msgs::Point punto_0;

punto_0.x = 0;
punto_0.y = 80;
punto_0.z = 50;

int inclinacion_pinza = 0;

brazo_fer::WriteServos inicio_brazo = inversa(punto_0, inclinacion_pinza, posicion_servos_0, 0);

brazo_fer::Servos posicion_servos_1;

posicion_servos_1.pinza = 511;

brazo_fer::WriteServos inicio_pinza = control_pinza(posicion_servos_1, posicion_servos_0, corriente_servos);

move_pub_.publish(inicio_brazo);

hand_pub_.publish(inicio_pinza);

return punto_0;

}

#endif

</syntaxhighlight>

===Programa===

Este programa ejecuta la secuencia para coger una botella pequeña de plástico (vacía, ya que la potencia de los servomotores es insuficiente para una llena, deberían emplearse más servomotores por articulación). Los pasos que realiza son los siguientes:

# Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).
# Posicionamiento en el punto indicado.
# Cierre de la pinza (hasta tener agarrada la botella).
# Levantamiento y acercamiento (levanta y acerca la botella al MYRAbot)

Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v02.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "math.h"

geometry_msgs::Point punto_0,punto_1;

int cont = 0;
int cont_1 = 0;
int cont_2 = 0;
int inclinacion_pinza = 0;
int start = 0;

brazo_fer::Servos pg, cg;
brazo_fer::WriteServos move;
brazo_fer::Servos pinza;
brazo_fer::WriteServos pin;
brazo_fer::WriteServos error_coordenadas;

brazo_fer::WriteServos acercar(geometry_msgs::Point cerca)
{
double x = cerca.x;
double z = cerca.z;

cerca.z = cerca.z - 70*cos(atan2(x,z));
cerca.x = cerca.x - 70*sin(atan2(x,z));

brazo_fer::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);

return move;

}

brazo_fer::WriteServos levantar_acercar(geometry_msgs::Point la)
{

la.x = 0;
la.y = la.y + 50;
la.z = 60;

if (la.y < 75) {
la.y = 75;
}

brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);

::punto_0 = la;

return move;

}

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

brazo_fer::Servos p = pec.posicion;

::pg = pec.posicion;
::cg = pec.corriente;

brazo_fer::Servos e = pec.estado;

brazo_fer::Servos c = pec.corriente;

if (::cont == 0)
{
::punto_0 = home(p, c);

::punto_1 = ::punto_0;

::cont = ::cont+1;

}

if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {

if (::cont_1 == 0) {

::move = acercar(::punto_0);

move_pub_.publish(::move);

::cont_1 = ::cont_1+1;

}

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

if (::cont_2 == 0) {

::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);

move_pub_.publish(::move);

::cont_2 = ::cont_2+1;

}
else {

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

::pinza.pinza = p.pinza;

if(::pin.posicion.pinza != p.pinza) {

std::cout<<c.pinza<<std::endl;

::pinza.pinza = ::pinza.pinza + 20;

::pin = control_pinza(::pinza, p, c);

hand_pub_.publish(::pin);

}
else {

std::cout<<"Agarrado"<<std::endl;

::move = levantar_acercar(::punto_0);

move_pub_.publish(::move);

::punto_1 = ::punto_0;

::cont_1 = 0;
::cont_2 = 0;
}
}
}

}
else {

std::cout<<"En movimiento"<<std::endl;
}

}

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); //Publicación del topic "move_arm"

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); //Publicación del topic "hand_arm"

::punto_0 = point;

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_recogida");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Ejecutando el programa===

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para ejecutar el programa de control llamado "control_v02", en un terminal nuevo ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>

Para poder publicar las coordenadas del punto donde se encuentra la botella (ya que no disponemos de otros medios para calcular la posición, se colocará la botella en una posición conocida), debemos publicar en el topic "pick_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella):

<syntaxhighlight>rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>

En el siguiente vídeo se puede ver la ejecución de la secuencia para la recogida de una botella:

<center><videoflash>0FcB6-i23cU</videoflash></center>

<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Ver vídeo en YouTube]</center>

==Quinto programa (posa la botella)==

Este programa ejecuta la secuencia para posar la botella que hemos cogido. Los pasos que realiza son los siguientes:

# Posicionamiento en el punto indicado.
# Apertura de la pinza (hasta soltar la botella).
# Separación (sitúa el brazo a unos 70 mm del punto indicado).
# Retorno a la posición de inicial.

Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v03.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "math.h"

geometry_msgs::Point punto_0, punto_1;

int cont = 0;
int cont_1 = 0;
int cont_2 = 0;
int cont_3 = 0;
int inclinacion_pinza = 0;

brazo_fer::Servos pg, cg, pinza;
brazo_fer::WriteServos move;
brazo_fer::WriteServos pin;
brazo_fer::WriteServos error_coordenadas;

brazo_fer::WriteServos separar(geometry_msgs::Point lejos)
{
double x = lejos.x;
double z = lejos.z;

lejos.z = lejos.z - 70*cos(atan2(x,z));
lejos.x = lejos.x - 70*sin(atan2(x,z));

brazo_fer::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);

return move;

}

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

brazo_fer::Servos p = pec.posicion;

::pg = pec.posicion;
::cg = pec.corriente;

brazo_fer::Servos e = pec.estado;

brazo_fer::Servos c = pec.corriente;

if (::cont == 0)
{
::punto_0 = directa(p, inclinacion_pinza);

::punto_1 = ::punto_0;

::cont = ::cont+1;
}

if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {

if (::cont_1 == 0) {

::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);

move_pub_.publish(::move);

::cont_1 = ::cont_1+1;

}

if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

if (::cont_2 == 0) {

::pinza.pinza = 470;

::pin = control_pinza(::pinza, p, c);

hand_pub_.publish(::pin);

::cont_2 = ::cont_2+1;

}
else {

if (p.pinza <= 495) {

if (::cont_3 == 0) {

::move = separar(::punto_0);

move_pub_.publish(::move);

::cont_3 = ::cont_3+1;

}

else if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {

std::cout<<"Suelto"<<std::endl;

::cont = 0;

::cont_1 = 0;
::cont_2 = 0;
::cont_3 = 0;
}
}
}

}
else {

std::cout<<"En movimiento"<<std::endl;
}
}

}

void punto(const geometry_msgs::Point& point)
{

::punto_0 = point;

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_entrega");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Ejecutando el programa===

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para ejecutar el programa de control llamado "control_v03", en un terminal nuevo ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>

Para poder publicar las coordenadas del punto donde queremos posar la botella, debemos publicar en el ''topic'' "place_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>

==Sexto programa (Sigue el camino recto)==

Se ha creado un programa que desplaza el brazo desde el punto en el que se encuentre al que le indiquemos, siguiendo la línea recta que une los puntos. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "control_v04.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "geometry_msgs/Point.h"
#include "math.h"

brazo_fer::Servos p, e, c;
brazo_fer::WriteServos destino, mover;
geometry_msgs::Point punto_0, punto_1, punto_i;
double lambda = 0;
int inclinacion_pinza = 0;
int cont = 0;

void posicion(const brazo_fer::ReadServos& pose)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

::p = pose.posicion;
::e = pose.estado;
::c = pose.corriente;

if (cont == 0)
{
::punto_0 = home(::p, ::c);

::punto_1 = ::punto_0;

cont = 1;
}

if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::mover.posicion.base && ::mover.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::mover.posicion.arti1 && ::mover.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::mover.posicion.arti2 && ::mover.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::mover.posicion.arti3 && ::mover.posicion.arti3 < (::p.arti3+15)))
{

if (((::p.base-15) < ::destino.posicion.base && ::destino.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.posicion.arti1 && ::destino.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.posicion.arti2 && ::destino.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.posicion.arti3 && ::destino.posicion.arti3 < (::p.arti3+15)))
{
::punto_0 = ::punto_1;
::lambda = 0;
}
else
{
geometry_msgs::Point u;

//ecuación de la recta p1 = p0+lambda.u

u.x = ::punto_1.x - ::punto_0.x;
u.y = ::punto_1.y - ::punto_0.y;
u.z = ::punto_1.z - ::punto_0.z;

double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);

::lambda = ::lambda + 1/paso;

::punto_i.x = ::punto_0.x + (::lambda)*u.x;
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
::punto_i.z = ::punto_0.z + (::lambda)*u.z;

::mover = inversa(::punto_i, ::inclinacion_pinza, ::p, 0);

move_pub_.publish(::mover);

}

}

}

void punto(const geometry_msgs::Point& point)
{
ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

::lambda = 0;

::punto_0 = directa(::p, ::inclinacion_pinza);

::punto_1 = point;

::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);

}

int main(int argc, char **argv)
{

ros::init(argc, argv, "control_brazo");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);

ros::Subscriber point_sub_= n.subscribe("point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Ejecutando el programa===

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para ejecutar el programa de control llamado "control_v04", en un terminal nuevo ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>

Para poder publicar las coordenadas del punto donde queremos que se deplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>

==Séptimo programa (Teleoperador)==

Se ha desarrollado un programa tele-operador para poder manejar el brazo en modo manual usando el teclado del PC. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "teleoperador_teclado.cpp" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "brazo_fer/brazo_fer.h"
#include "geometry_msgs/Point.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_Xmas 0x43
#define KEYCODE_Xmenos 0x44
#define KEYCODE_Ymas 0x41
#define KEYCODE_Ymenos 0x42
#define KEYCODE_Zmas 0x77
#define KEYCODE_Zmenos 0x73
#define KEYCODE_Pabrir 0x61
#define KEYCODE_Pcerrar 0x64
#define KEYCODE_PinclinarMas 0x65
#define KEYCODE_PinclinarMenos 0x71

struct termios cooked, raw;

int cont = 0;

double retardo = 0.11;

geometry_msgs::Point punto;
brazo_fer::Servos vel;
brazo_fer::Servos pinza, p, c;

int pinza_incli = 0;

void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
{

::p = pec.posicion;
::c = pec.corriente;

}

void quit(int sig)
{
tcsetattr(0, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}

void callback(const ros::TimerEvent&)
{

ros::NodeHandle n;

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

ros::Time anterior;

brazo_fer::WriteServos teleop;
brazo_fer::WriteServos teleop_pinza;

if (::cont == 0)
{
anterior = ros::Time::now();

::punto = home(::p, ::c);

::cont = 1;

::pinza.pinza = 511;

teleop = inversa(::punto, ::pinza_incli, ::p, 0);
teleop_pinza = control_pinza(::pinza, ::p, ::c);

//std::cout<<teleop<<"-"<<teleop_pinza<<std::endl;
}

char c;

// get the console in raw mode
tcgetattr(0, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(0, TCSANOW, &raw);

puts("");
puts("#####################################################");
puts(" CONTROLES BRAZO");
puts("#####################################################");
puts("");
puts("Flecha arriba:________ Subir pinza");
puts("Flecha abajo:_________ Bajar pinza");
puts("Flecha izquierda:_____ Desplazar pinza a la izquierda");
puts("Flecha derecha:_______ Desplazar pinza a la derecha");
puts("Tecla W:______________ Desplazar pinza hacia delante");
puts("Tecla S:______________ Desplazar pinza hacia atrás");
puts("Tecla A:______________ Abrir pinza");
puts("Tecla D:______________ Cerrar pinza");
puts("Tecla Q:______________ Inclinar pinza hacia arriba");
puts("Tecla E:______________ Inclinar pinza hacia abajo");

while (ros::ok())
{

// get the next event from the keyboard
if(read(0, &c, 1) < 0)
{
perror("read():");
exit(-1);
}

if (c == KEYCODE_Xmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.x = ::punto.x - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.x = ::punto.x + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Xmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.x = ::punto.x + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.x = ::punto.x - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.y = ::punto.y + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.y = ::punto.y - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.y = ::punto.y - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.y = ::punto.y + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.z = ::punto.z + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.z = ::punto.z - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::punto.z = ::punto.z - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::punto.z = ::punto.z + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Pabrir)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza.pinza = ::pinza.pinza - 5;
teleop_pinza = control_pinza(::pinza, ::p, ::c);
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Pcerrar)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza.pinza = ::pinza.pinza + 5;
teleop_pinza = control_pinza(::pinza, ::p, ::c);
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_PinclinarMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza_incli = ::pinza_incli + 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::pinza_incli = ::pinza_incli - 5;}
anterior = ros::Time::now();
}
}
if (c == KEYCODE_PinclinarMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::pinza_incli = ::pinza_incli - 5;
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
{::pinza_incli = ::pinza_incli + 5;}
anterior = ros::Time::now();
}
}

if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
{
move_pub_.publish(teleop);
}
if (teleop_pinza.posicion.pinza != ::p.pinza)
{
hand_pub_.publish(teleop_pinza);
}

}
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_teclado");

ros::NodeHandle n;

ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);

signal(SIGINT,quit);

ros::Timer timer = n.createTimer(ros::Duration(1), callback);

ros::spin();

return 0;
}

</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>roscd brazo_fer
make</syntaxhighlight>

===Ejecutando el programa===

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roscore</syntaxhighlight>

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:

<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>

Para ejecutar el programa de control llamado "teleoperador_teclado", en un terminal nuevo ejecutaremos el siguiente comando:

<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</syntaxhighlight>

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

[[Mobile manipulation | < volver a principal]]

CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

2014-09-29T11:00:44Z

Fernando:

[[Mobile manipulation | < go back to main]]

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm and base control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

=Teleoperation of CeRVaNTeS=

We need a teleoperation system in order to take the control of the robot to make specific tasks. Currently we use the [http://es.wikipedia.org/wiki/Secure_Shell ssh] connection between the robot PC and our PC, using the keyboard. This method use a [http://es.wikipedia.org/wiki/Wi-Fi wifi] network that can fail or not available. For this reason the best option is to use a device plugged in to the PC robot with its own network. The well-chosen device is the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 wireless controller of microsoft].

=xbox360 controller=

==Description of buttons and axes==

This controller has 6 axes and 14 buttons that are distributed like is shown below:

* Left pad: 2 axes and 1 button.
* Right pad: 2 axes and 1 button.
* Left trigger: 1 axe.
* Right trigger: 1 axe.
* Cross: 4 buttons.
* Left side buttons: 4 buttons.
* Left button: 1 button.
* Right button: 1 button.
* "Back" button: 1 button.
* "Start" button: 1 button.

The function assigned to the buttons and axes of the [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for teleoperation of CeRVaNTeS is shown in the next picture:

[[file:xbox360_control-CeRVaNTeS-EN.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows xbox360 controller] for CeRVaNTeS.]]

==Program==

The program assigns the function to the buttons and axes. The program subscribes to the ''topic'' "joy" where is published the state of the buttons and axes and publishes the ''topics'' "Base_Control" and "Group_Motor_Control" for the control of the base and the [[CeRVaNTeS' arm control (maxon+epos2)|arm]]. We have to execute the next command in a terminal in order to create a new ''package'' named "cervantes_teleop" within our catkin workspace:

<syntaxhighlight>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

We will create a new file named "teleop_xbox360.cpp" within the folder "src" of the created ''package'' with the content that is shown below:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

We will add the next code line to the file "CMakeLists.txt" of the created ''package'' where we indicate the name of the executable and the path of the file to compile:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

We will execute the next command in a terminal, placed in the root folder of the our catkin work space, in order to compile and generate the executable:

<syntaxhighlight>roscd myrabot_teleop
make</syntaxhighlight>

We will create a file named "teleop_xbox360.launch" within the folder "launch" of the created ''package'' with the content that is shown below in order to start all the necessary programs:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

We will execute the next command in a terminal in order to start the teleoperator:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

In the next video we can see de teleoperation of CeRVaNTeS in the [http://gazebosim.org/ gazebo] simulator:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Related articles
----
!Other articles
----
|-
| valign="top" width="50%" |

[[CeRVaNTeS' arm control (maxon+epos2)]] 
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] 
[[CeRVaNTeS model for simulation (urdf+gazebo)]] 
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[MYRAbot's arm and base control (bioloid+arduino)]] 
[[Objects recognition and position calculation (webcam)]] 
[[MYRAbot's arm model for simulation (urdf+gazebo)]] 
[[MYRAbot model for simulation (urdf+gazebo)]] 
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]] 
[[Voice control (sphinx+festival)]] 
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]

|}
----

[[Mobile manipulation | < go back to main]]

Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)

2014-09-29T10:59:38Z

Fernando:

CeRVaNTeS model for simulation (urdf+gazebo)

2014-09-29T10:59:11Z

Fernando:

CeRVaNTeS' arm model for simulation (urdf+gazebo)

2014-09-29T10:58:38Z

Fernando:

Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

2014-09-29T10:58:13Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Teleoperación de CeRVaNTeS=

Para la realización de determinadas tareas es necesario hacer uso de algún sistema de teleoperación para controlar el robot. Lo más habitual es haciendo uso de una conexión [http://es.wikipedia.org/wiki/Secure_Shell ssh] con el PC del robot y usando el teclado de nuestro PC. Este método emplea una red [http://es.wikipedia.org/wiki/Wi-Fi wifi] de la cual depende la conexión entre los dos equipos, la cual puede fallar o ser inexistente. Por eso la mejor opción es emplear un dispositivo conectado al PC del robot provisto de su propia red inalámbrica. El dispositivo escogido por robustez y versatilidad ha sido el [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador inalámbrico xbox360 de microsoft].

=Controlador xbox360=

==Descripción de botones y ejes==

Este controlador está equipado con 6 ejes de control y 14 botones, que se reparten de la siguiente manera:

* Palanca izquierda: 2 ejes y 1 botón (pulsación).
* Palanca derecha: 2 ejes y 1 botón (pulsación).
* Gatillo izquierdo: 1 eje.
* Gatillo derecho: 1 eje.
* Cruz: 4 botones.
* Botones lado derecho: 4 botones.
* Botón izquierdo: 1 botón.
* Botón derecho: 1 botón.
* Botón "back": 1 botón.
* Botón "start": 1 botón.

En la siguiente imagen se muestra la función asignada a cada botón y eje del [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador xbox360] para la teleoperación de CeRVaNTeS:

[[file:xbox360_control-CeRVaNTeS.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador xbox360] para CeRVaNTeS.]]

==Programa==

Se encargará de la asignación de acciones a los diferentes botones y ejes del controlador. El programa se subscribe al ''topic'' "joy" donde se publica el estado de los botones del controlador y publica los ''topics'' "Base_Control" y "Group_Motor_Control" para el control de la base y el [[Control brazo CeRVaNTeS (maxon+epos2)|brazo]]. Comenzaremos creando un nuevo ''package'' en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo catkin y ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight enclose=div>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

Crearemos un archivo llamado "teleop_xbox360.cpp" dentro del directorio "src" del ''package'' creado con el siguiente contenido:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir las siguientes líneas de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio raiz de nuestro espacio de trabajo catkin. Simplemente con ejecutar el siguiente comando en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>catkin_make</syntaxhighlight>

Crearemos un archivo llamado "teleop_xbox360.launch" dentro del directorio "launch" del ''package'' creado con el siguiente contenido, para poder iniciar en una sola ejecución todos los programas necesarios:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

Para iniciar el teleoperador simplemente deberemos ejecutar en un terminal el siguiente comando:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

En el siguiente vídeo puede verse el uso del teleoperador para CeRVaNTeS en el simulador [http://gazebosim.org/ gazebo]:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

[[Mobile manipulation | < volver a principal]]

Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

2014-09-29T10:55:36Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Teleoperación de CeRVaNTeS=

Para la realización de determinadas tareas es necesario hacer uso de algún sistema de teleoperación para controlar el robot. Lo más habitual es haciendo uso de una conexión [http://es.wikipedia.org/wiki/Secure_Shell ssh] con el PC del robot y usando el teclado de nuestro PC. Este método emplea una red [http://es.wikipedia.org/wiki/Wi-Fi wifi] de la cual depende la conexión entre los dos equipos, la cual puede fallar o ser inexistente. Por eso la mejor opción es emplear un dispositivo conectado al PC del robot provisto de su propia red inalámbrica. El dispositivo escogido por robustez y versatilidad ha sido el [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador inalámbrico xbox360 de microsoft].

=Controlador xbox360=

==Descripción de botones y ejes==

Este controlador está equipado con 6 ejes de control y 14 botones, que se reparten de la siguiente manera:

* Palanca izquierda: 2 ejes y 1 botón (pulsación).
* Palanca derecha: 2 ejes y 1 botón (pulsación).
* Gatillo izquierdo: 1 eje.
* Gatillo derecho: 1 eje.
* Cruz: 4 botones.
* Botones lado derecho: 4 botones.
* Botón izquierdo: 1 botón.
* Botón derecho: 1 botón.
* Botón "back": 1 botón.
* Botón "start": 1 botón.

En la siguiente imagen se muestra la función asignada a cada botón y eje del [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador xbox360] para la teleoperación de CeRVaNTeS:

[[file:xbox360_control-CeRVaNTeS.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador xbox360] para CeRVaNTeS.]]

==Programa==

Se encargará de la asignación de acciones a los diferentes botones y ejes del controlador. El programa se subscribe al ''topic'' "joy" donde se publica el estado de los botones del controlador y publica los ''topics'' "Base_Control" y "Group_Motor_Control" para el control de la base y el [[Control brazo CeRVaNTeS (maxon+epos2)|brazo]]. Comenzaremos creando un nuevo ''package'' en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo catkin y ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight enclose=div>catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cpp</syntaxhighlight>

Crearemos un archivo llamado "teleop_xbox360.cpp" dentro del directorio "src" del ''package'' creado con el siguiente contenido:

<syntaxhighlight lang=cpp enclose=div>
#include "ros/ros.h"
#include <cervantes_epos_manager/GroupEPOSControl.h>
#include <cervantes_epos_manager/EPOSControl.h>
#include <cervantes_epos_manager/GroupMotorInfo.h>
#include <cervantes_epos_manager/MotorInfo.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Joy.h"
#include "math.h"

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define PI 3.14159265

double avance = 0.005, giro = 0.05;
double avance_base = 0.05, giro_base = 0.1;
int start = 0;
int flanco = 0;

geometry_msgs::Twist base;

double pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;

void xbox(const sensor_msgs::Joy& xbox_joystick)
{

::pad_izquierda_x = xbox_joystick.axes[0];
::pad_izquierda_y = xbox_joystick.axes[1];
::gatillo_izquierda = xbox_joystick.axes[2] -1;
::pad_derecha_x = xbox_joystick.axes[3];
::pad_derecha_y = xbox_joystick.axes[4];
::gatillo_derecha = xbox_joystick.axes[5] -1;

::boton_a = xbox_joystick.buttons[0];
::boton_b = xbox_joystick.buttons[1];
::boton_x = xbox_joystick.buttons[2];
::boton_y = xbox_joystick.buttons[3];
::boton_izquierda = xbox_joystick.buttons[4];
::boton_derecha = xbox_joystick.buttons[5];
::boton_back = xbox_joystick.buttons[6];
::boton_start = xbox_joystick.buttons[7];
::boton_pad_izquierda = xbox_joystick.buttons[9];
::boton_pad_derecha = xbox_joystick.buttons[10];
::cruz_izquierda = xbox_joystick.buttons[11];
::cruz_derecha = xbox_joystick.buttons[12];
::cruz_arriba = xbox_joystick.buttons[13];
::cruz_abajo = xbox_joystick.buttons[14];
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "teleoperador_xbox360");

ros::NodeHandle n;

ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);

ros::Publisher move_pub_=n.advertise<cervantes_epos_manager::GroupEPOSControl>("Group_Motor_Control", 1);

ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("Base_Control", 1);

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("brazo");

std::vector<double> joint_values;

std::vector<std::string> joint_names;

joint_names = joint_model_group->getJointModelNames();

Eigen::Affine3d end_effector_state, end_effector_state_i;

int cont = 0;

ros::Rate loop_rate(5);

while (ros::ok())
{

end_effector_state = kinematic_state->getGlobalLinkTransform("muneca_c_link");

if (cont == 0)
{
end_effector_state_i = end_effector_state;

cont = 1;
}
else
{
if (::boton_start != 0 && ::flanco == 0)
{
flanco = 1;
}
else
{
if (::boton_start != 0)
{
::flanco = 2;
}
else
{
::flanco = 0;
}
}

if (::boton_start != 0 && ::flanco == 1)
{
if (::start == 0)
{
::start = 1;
}
else
{
::start = 0;
}
}

if (::start == 1)
{

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 ||
(::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) ||
(::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) ||
::cruz_izquierda != 0 || ::cruz_derecha != 0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 ||
::boton_back != 0 ||
::boton_izquierda != 0 || ::boton_derecha != 0)
{

if (::boton_back != 0)
{
end_effector_state = end_effector_state_i;
}

if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, 0, ::pad_derecha_x * avance));
}

if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
{
end_effector_state.translate(Eigen::Vector3d(0, -::pad_derecha_y * avance, 0));
}

if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
{
end_effector_state.translate(Eigen::Vector3d(::gatillo_izquierda * (avance/2), 0, 0));
}

if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
{
end_effector_state.translate(Eigen::Vector3d(-::gatillo_derecha * (avance/2), 0, 0));
}

if (::cruz_arriba != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_abajo != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitZ()));
}

if (::cruz_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitY()));
}

if (::cruz_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitY()));
}

if (::boton_derecha != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(giro, Eigen::Vector3d::UnitX()));
}

if (::boton_izquierda != 0)
{
end_effector_state.rotate(Eigen::AngleAxisd(-giro, Eigen::Vector3d::UnitX()));
}

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 5, 0.1);

if (found_ik)
{
cervantes_epos_manager::GroupEPOSControl arm;
std::vector<cervantes_epos_manager::EPOSControl> motor_group;
cervantes_epos_manager::EPOSControl motor_control;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

for(int i = 0; i < joint_names.size(); i++)
{
if (joint_names[i] == "hombro_a")
{
motor_control.node_id = 3;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "hombro_b")
{
motor_control.node_id = 5;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_a")
{
motor_control.node_id = 4;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "codo_b")
{
motor_control.node_id = 6;
motor_control.setpoint = (joint_values[i]/(2*PI))*(246*1024);
}
else if (joint_names[i] == "muneca_a")
{
motor_control.node_id = 7;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*1024);
}
else if (joint_names[i] == "muneca_b")
{
motor_control.node_id = 9;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}
else if (joint_names[i] == "muneca_c")
{
motor_control.node_id = 8;
motor_control.setpoint = (joint_values[i]/(2*PI))*(270*16);
}

motor_control.control_mode = 3;
motor_group.push_back(motor_control);

}

arm.motor_group = motor_group;

move_pub_.publish(arm);

}
else
{
ROS_INFO("Did not find IK solution");
}

}
else
{
if (::boton_a != 0)
{
::avance_base = ::avance_base - 0.01;

if (::avance_base < 0.01)
{
::avance_base = 0.01;
}
}

if (::boton_b != 0)
{
::avance_base = ::avance_base + 0.1;

if (::avance_base > 0.1)
{
::avance_base = 0.1;
}
}

if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
{
::base.linear.x = ::avance_base * ::pad_izquierda_y;
base_pub_.publish(::base);
}
else
{
::base.linear.x = 0.0;
base_pub_.publish(::base);
}

if (::boton_x != 0)
{
::giro_base = ::giro_base - 0.01;

if (::giro_base < 0.05)
{
::giro_base = 0.05;
}
}

if (::boton_y != 0)
{
::giro_base = ::giro_base + 0.01;

if (::giro_base > 1)
{
::giro_base = 1;
}
}

if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
{
::base.angular.z = ::giro_base * ::pad_izquierda_x;
base_pub_.publish(::base);
}
else
{
::base.angular.z = 0.0;
base_pub_.publish(::base);
}
}
}
}

ros::spinOnce();

loop_rate.sleep();
}

return 0;
}
</syntaxhighlight>

Para compilarlo y generar el ejecutable se debe añadir las siguientes líneas de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:

<syntaxhighlight lang=cmake>add_executable(teleop_xbox360 src/teleop_xbox360.cpp)

target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)</syntaxhighlight>

Para compilar el programa hay que situarse en el directorio raiz de nuestro espacio de trabajo catkin. Simplemente con ejecutar el siguiente comando en un terminal se compilará y creará el ejecutable, siempre que no existan errores:

<syntaxhighlight>catkin_make</syntaxhighlight>

Crearemos un archivo llamado "teleop_xbox360.launch" dentro del directorio "launch" del ''package'' creado con el siguiente contenido, para poder iniciar en una sola ejecución todos los programas necesarios:

<syntaxhighlight lang=xml>

<launch>

<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />

<node pkg="joy" type="joy_node" name="joystick"/>

<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />

</launch>

</syntaxhighlight>

Para iniciar el teleoperador simplemente deberemos ejecutar en un terminal el siguiente comando:

<syntaxhighlight>roslaunch cervantes_teleop teleop_xbox360.launch</syntaxhighlight>

En el siguiente vídeo puede verse el uso del teleoperador para CeRVaNTeS en el simulador [http://gazebosim.org/ gazebo]:

<center><videoflash>wF_gKr1KWfg#t=13</videoflash></center>

<center>[http://www.youtube.com/watch?v=wF_gKr1KWfg#t=13 Ver vídeo en YouTube]</center>

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

[[Mobile manipulation | < volver a principal]]

Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)

2014-09-29T10:55:11Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Pasos previos=

==Instalación de moveit==

=Configuración de CeRVaNTeS en moveIt!=

=Ejecución=

==Planificación simple==

[[file:CeRVaNTeS_rviz_moveit.png|thumb|500px|center|CeRVaNTeS en [http://wiki.ros.org/rviz rviz] con ''plugin'' de [http://moveit.ros.org/ moveIt!]]]

<center><videoflash>myGdut2WdsE</videoflash></center>

<center>[http://www.youtube.com/watch?v=myGdut2WdsE Ver vídeo en YouTube]</center>

<center><videoflash>9hV3UANRs2k</videoflash></center>

<center>[http://www.youtube.com/watch?v=9hV3UANRs2k Ver vídeo en YouTube]</center>

<center><videoflash>bYtvNJHraVs</videoflash></center>

<center>[http://www.youtube.com/watch?v=bYtvNJHraVs Ver vídeo en YouTube]</center>

[[Mobile manipulation | < volver a principal]]

Modelo para simulación CeRVaNTeS (urdf+gazebo)

2014-09-29T10:54:34Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Componentes de modelo de CeRVaNTeS=

==URDF base móvil y estructura==

=Ensamblado del modelo de CeRVaNTeS=

==Visualización del modelo en rviz==

[[file:CeRVaNTeS_rviz.png|thumb|500px|center|Modelo CeRVaNTeS en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]

=Carga de modelo en gazebo=

[[file:CeRVaNTeS_gazebo.png|thumb|500px|center|Modelo CeRVaNTeS en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

[[Mobile manipulation | < volver a principal]]

Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)

2014-09-29T10:53:55Z

Fernando:

[[Mobile manipulation | < volver a principal]]

----
{| style="border: solid 0px white; width: 100%"
!Artículos realcionados
----
!Otros artículos
----
|-
| valign="top" width="50%" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]] 
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] 
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] 
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]] 
[[Detección y cálculo de posición de objetos (cámara web)]] 
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] 
[[Modelo para simulación MYRAbot (urdf+gazebo)]] 
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] 
[[Órdenes y confirmación mediante voz (sphinx+festival)]] 
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----

=Creación del modelo URDF=

=Prueba visual del modelo=

[[file:CeRVaNTeS_arm_rviz.png|thumb|500px|center|Modelo cargado en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]

=Control de uniones para simulación=

==Pasos previos==

==Añadir controladores a uniones móviles==

===Modificación del URDF===

===Creación de los archivos de configuración===

==Carga del modelo en gazebo==

[[file:CeRVaNTeS_arm_gazebo.png|thumb|500px|center|Modelo brazo CeRVaNTeS en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]

[[Mobile manipulation | < volver a principal]]

CeRVaNTeS' arm and base control (maxon+epos2)

2014-09-29T10:51:00Z

Fernando:

CeRVaNTeS' arm and base control (maxon+epos2)

2014-09-29T10:50:43Z

Fernando: Created page with " < go back to main ---- {| style="border: solid 0px white; width: 100%" !Related articles ---- !Other articles ---- |- | valign="top" width="50%" | ..."