Difference between revisions of "Fernando-TFM-ROS02"

Revision as of 10:50, 28 February 2012

• Project Name: TFM-ROS02

• Authors: Fernando Casado

• Web of Authors: http://fcg.adamelo.es

• Academic Year: 2011-2012

• Degree: Master

• SVN Repositories: http://robotica.unileon.es/svn/Projects/PFC/FernandoTFMROS02/

• Tags: Turtlebot

• Technology: ROS, c++

• State: Development

Instalando ROS

Lo primero que necesitamos es un computador con el sistema operativo Ubuntu, que es el único soportado por ROS, o instalarlo; otros sistemas operativos para los que está disponible solo son experimentales. Para instalar 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 Ubuntu, para descargar e instalar el sistema ROS simplemente deberemos seguir las indicaciones de instalación. No esta de más el tomarse un tiempo realizando los 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 ROS instalado, necesitamos instalar las herramientas para desarrollo del robot Turtlebot en el equipo de trabajo. Ejecutando el siguiente comando en un terminal comenzará la descarga e instalación:

   
     sudo apt-get install ros-electric-turtlebot-desktop

Es importante que el reloj del equipo de trabajo se encuentre sincronizado con el reloj del equipo del Turtlebot, para prevenir las posibles perdidas de red inalámbrica. Para instalar el cliente NTP Chrony se ejecutará el siguiente comando en un terminal:

      sudo apt-get install chrony

Conociendo el Turtlebot

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

Hardware

• iRobot Create
• ASUS1215N Laptop
• Microsoft Kinect
• Placa de gestión de alimentación y sensores
• Cables USB de comunicaciones y alimentación
• Estructura de soporte de elementos

Software

• Sistema operativo Ubuntu
• ROS con Turtlebot-robot

Conexión estación de trabajo-robot

Configuración de la red

Lo habitual conectar ambos equipos a una red 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 router que provee la red 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ñarirá 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:

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

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 de trabajo se ejecutará el siguiente comando, sustituyendo "IP_OF_TURTLEBOT", por la dirección IP asignada en la red al PC robot:

      
      echo export ROS_HOSTNAME=IP_OF_TURTLEBOT >> ~/.bashrc

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:

      
      echo export ROS_HOSTNAME=IP_OF_WORKSTATION >> ~/.bashrc

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

      
      ifconfig

que mostrará una información similar a esta:

      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)

Puesta en marcha

Con la red configurada, ya se puede proceder a la puesta en marcha del Turtlebot. Se conectarán los cables USB de comunicación y alimentación del robot al PC robot, se encenderá el 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 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:

      ssh turtlebot@IP_OF_TURTLEBOT

Pedirá la contraseña asignada al PC robot. Cuando se conecte el 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:

      sudo service turtlebot status

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

      sudo service turtlebot stop
      sudo service turtlebot start

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:

      rosrun turtlebot_dashboard turtlebot_dashboard&

Aparecerá una pequeña ventana similar a esta:

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 iRobot Create.
• Niveles de batería de el 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).

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 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 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:

      mkdir ~/ros_workspace

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:

      #!/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

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:

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

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:

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

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:

      <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>

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:

      rosmake turtlebot_fer

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 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 "turtlebot_node/cmd_vel", que hace mover el robot con una velocidad de avance y de giro, hasta que se reciba la señal de parada "Ctrl+C". El programa es el siguiente:

#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>("turtlebot_node/cmd_vel", 1);   //Publicación del topic "turtlebot_node/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;
}

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:

      rosbuild_add_executable(gira src/gira.cpp)

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:

      roscd turtlebot_fer
      make

Ejecutando el programa

Simulador

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 lineas 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:

      # export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311
      # export ROS_HOSTNAME=IP_OF_WORKSTATION

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

      roslaunch turtlebot_gazebo turtlebot_empty_world.launch

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

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:

      rosrun turtlebot_fer gira

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.

Conectado el PC estación de trabajo al PC robot, se debe crear un espacio de trabajo y 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 nuevo terminal local:

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

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

      rosrun turtlebot_fer gira

• Nota: si se encuentra conectada al robot alguna de las fuentes de alimentación, PC robot o 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 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:

#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>("turtlebot_node/cmd_vel", 1);   //Publicación del topic "turtlebot_node/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>("turtlebot_node/cmd_vel", 1);   //Publicación del topic "turtlebot_node/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;
}

Si se tiene ya creado el espacio de trabajo y el package, simplemente se debe guardar en un archivo llamado "juguete_golpe.cpp", compilar y ejecutar.

• Nota: El modelo 3D del Turtlebot empleado en el simulador no implementa este sensor, por lo que no será posible probar el programa en el simulador.

Tercer programa (Sigue el rojo)

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 "pixeles ancho x pixeles 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 pixel, esquina inferior derecha de la imagen.

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,2 %, de los píxeles totales de la imagen, e inferior al 30 %, comienza el seguimiento hasta hacer el numero de píxeles totales cercano al 15 % e igual a cada lado. Cuando el objeto se encuentre demasiado lejos o demasiado cerca se detendrá el seguimiento. El código del programa, escrito en lenguaje c++, es el siguiente:

#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"

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

void callback(const sensor_msgs::Image& ima)
{	
	ros::NodeHandle n, nh;
  
  	ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("turtlebot_node/cmd_vel", 1);   //Publicación del topic "turtlebot_node/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;

        ::rojo = ima;   //Asignación del valor de ima a la variable global rojo
	
	for (uint i = 0; i < (tam - (ancho/2)); i = i + ancho)  
	{
		for (uint j = 0; j < (ancho/2); 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 > 80 && pix_g < 30 && pix_b < 60)   //Contar píxeles tonos rojos lado izquierdo imagen
			{
				cont_izq++;
			}
		}
	}
	
	for (uint i = (ancho/2); i < tam; i = i + ancho)
	{
		for (uint j = 0; j < (ancho/2); 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 > 80 && pix_g < 30 && pix_b < 60)   //Contar píxeles tonos rojos lado derecho imagen
			{
				cont_der++;
			}
		}
	}

	double cont = cont_izq + cont_der;

	if (cont > ((tam/3) * 0.002) && cont < ((tam/3) * 0.3))   //Comprobación si se está dentro del limite inferior y superior de píxeles
	{
		::vel.angular.z = (cont_izq - cont_der) / ((tam/3) * 0.1);   //Velocidad de giro
		::vel.linear.x = (((tam/3) * 0.15) - cont) / ((tam/3) * 0.85);   //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>("turtlebot_node/cmd_vel", 1);   //Publicación del topic "turtlebot_node/cmd_vel"
  	
  	ros::Subscriber image_sub_= n.subscribe("camera/rgb/image_color", 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;
}

• Nota: El correcto funcionamiento queda condicionado a la iluminación del recinto, ya que pueden existir grandes fluctuaciones de tonalidad del objeto debidas a la iluminación, provocando un comportamiento errático.

Si se tiene ya creado el espacio de trabajo y el package, simplemente se debe guardar en un archivo llamado "sigue_rojo.cpp", compilar y ejecutar. Para el uso de la cámara Kinect se debe lanzar en el PC robot el "kinect.launch", ejecutando el siguiente comando en un terminal:

      roslaunch turtlebot_bringup kinect.launch

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:

#include "ros/ros.h"   //Librerí­a de funciones del sistema ROS
#include "sensor_msgs/Image.h"   //Librería "Image" del package "sensor_msgs"
 
sensor_msgs::Image rojo;   //Declaración de la variable global rojo
 
void callback(const sensor_msgs::Image& ima)
{	
	ros::NodeHandle nh;
 
	ros::Publisher rojo_pub_=n.advertise<sensor_msgs::Image>("camara/rojo", 1);   //Publicación del topic "camara/rojo"   //Publicación de la imagen "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
 
	::rojo = ima;   //Asignación del valor de ima a la variable global rojo
 
	for (uint i = 0; i < (tam - (ancho/2)); i = i + ancho)  
	{
		for (uint j = 0; j < (ancho/2); j = j + 3)
		{
			pix_r = ima.data[i + j];
			pix_g = ima.data[i + j + 1];
			pix_b = ima.data[i + j + 2];
 
			if (j == ((ancho/2)-3))   //Línea negra divisoria lado izquierdo
			{
				::rojo.data[i + j] = 0;
				::rojo.data[i + j + 1] = 0;
				::rojo.data[i + j + 2] = 0;
			}
 
			else if (pix_r > 80 && pix_g < 30 && pix_b < 60)   //Contar y colorear pí­xeles tonos rojos lado izquierdo imagen
			{
				::rojo.data[i + j] = 255;
				::rojo.data[i + j + 1] = 0;
				::rojo.data[i + j + 2] = 0;
			}
 
			else   //Conversión a escala de grises resto pí­xeles lado izquierdo imagen
			{
				if (pix_r > pix_g)
				{
					::rojo.data[i + j] = pix_r;
					::rojo.data[i + j + 1] = pix_r;
					::rojo.data[i + j + 2] = pix_r;
				}
				else if (pix_g > pix_b)
				{
					::rojo.data[i + j] = pix_g;
					::rojo.data[i + j + 1] = pix_g;
					::rojo.data[i + j + 2] = pix_g;
				}
				else
				{
					::rojo.data[i + j] = pix_b;
					::rojo.data[i + j + 1] = pix_b;
					::rojo.data[i + j + 2] = pix_b;
				}
			}
		}
	}
 
	for (uint i = (ancho/2); i < tam; i = i + ancho)
	{
		for (uint j = 0; j < (ancho/2); j = j + 3)
		{
			pix_r = ima.data[i + j];
			pix_g = ima.data[i + j + 1];
			pix_b = ima.data[i + j + 2];
 
			if (j == 0)    //Línea negra divisoria lado derecho
			{
				::rojo.data[i + j] = 0;
				::rojo.data[i + j + 1] = 0;
				::rojo.data[i + j + 2] = 0;
			}		
 
			else if (pix_r > 80 && pix_g < 30 && pix_b < 60)   //Contar y colorear pí­xeles tonos rojos lado derecho imagen
			{
				::rojo.data[i + j] = 255;
				::rojo.data[i + j + 1] = 0;
				::rojo.data[i + j + 2] = 0;
			}
			else   //Conversión a escala de grises resto píxeles lado derecho imagen
			{
				if (pix_r > pix_g)
				{
					::rojo.data[i + j] = pix_r;
					::rojo.data[i + j + 1] = pix_r;
					::rojo.data[i + j + 2] = pix_r;
				}
				else if (pix_g > pix_b)
				{
					::rojo.data[i + j] = pix_g;
					::rojo.data[i + j + 1] = pix_g;
					::rojo.data[i + j + 2] = pix_g;
				}
				else
				{
					::rojo.data[i + j] = pix_b;
					::rojo.data[i + j + 1] = pix_b;
					::rojo.data[i + j + 2] = pix_b;
				}
			}	
		}
	}
 
        rojo_pub_.publish(::rojo);
}
 
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 "turtlebot_node/sensor_state"
 
	ros::spin();   //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"
 
	return 0;
}

Si se tiene ya creado el espacio de trabajo y el package, simplemente se debe guardar en un archivo llamado "sigue_rojo.cpp", compilar y ejecutar.

Para poder ejecutar el programa "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:

      <launch>
        <node name="sigue_rojo" pkg="turtlebot_fer" type="sigue_rojo" />
        <node name="visualizacion" pkg="turtlebot_fer" type="visualizacion" />
      </launch>

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:

      roslaunch turtlebot_fer rojo.launch

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:

      rosrun image_view image_view image:=/camara/rojo

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 imagenes obtenidas tendrán un aspecto similar al siguiente:

center