View source for Detección y cálculo de posición de objetos (cámara web)

[[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)]]<br/>
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----


=Detección de objetos=

Para esta tarea se ha empleado el ''package'' [http://code.google.com/p/find-object/ find_object_2d] desarrollado por [https://introlab.3it.usherbrooke.ca/mediawiki-introlab/index.php/Mathieu_Labb%C3%A9 Mathieu Labbé] (Université de Sherbrooke ). Se trata de una aplicación con entorno gráfico que permite detectar objetos presentes en la escena, de los cuales ha sido previamente guardada una imagen.

[[file:find_object_2d.png|thumb|500px|center|Captura [http://code.google.com/p/find-object/ find_object_2d]]]

En la captura del entorno gráfico podemos observar como aparece enmarcado el objeto encontrado en la escena. Este marco se sitúa en las esquinas del objeto a partir de la imagen guardada. El entorno dispone de un menú para añadir objetos a partir de objetos presentes en la escena, tomando una imagen y seleccionando el área donde se encuentra el objeto. Estos objetos añadidos desde las escena podemos guardarlos para volver a usarlos.

La aplicación proporciona diversos datos de cada objeto encontrado en la escena, pero vamos a utilizar los siguientes: 

* Ancho de la imagen del objeto (número de píxeles).
* Alto de la imagen del objeto (número de píxeles).
* Posición de las esquinas de la imagen del objeto en la escena.

==Utilización de una nueva versión de OpenCV en ROS Electric==

Para poder compilar el anterior ''package'' necesitamos utilizar la versión 2.4.3, o posterior, de la librería [http://opencv.org/ OpenCV], pero en la versión "[http://wiki.ros.org/electric/Installation electric]" de [http://wiki.ros.org/ ROS] la versión que viene integrada de esta librería es la 2.3.1. Por esto vamos a instalar la versión 2.4.3 de [http://opencv.org/ OpenCV], configurar el ''package'' [http://wiki.ros.org/cv_bridge?distro=electric cv_bridge] y modificar el ''package'' [http://code.google.com/p/find-object/ find_object_2d] para que utilice esta nueva versión de la librería.

===Instalación de OpenCV===

Lo primero vamos a [http://opencv.org/downloads.html Descargar] la versión 2.4.3 de [http://opencv.org/ OpenCV] de la página oficial. Descomprimiremos el archivo descargado y situaremos el contenido en el emplazemiento que más nos guste. Los primero que vamos a realizar  es una actualización de los paquetes de [http://www.ubuntu.com/ ubuntu], para lo que ejecutaremos en un terminal los siguientes comandos:

<syntaxhighlight>
sudo apt-get update
sudo apt-get upgrade
</syntaxhighlight>

Realizada la actualización vamos a continuar compilando [http://opencv.org/ OpenCV]. Para esto, desde un terminal, vamos a situarnos dentro del directorio "OpenCV-2.4.3", carpeta por defecto al descomprimir el archivo con [http://opencv.org/ OpenCV]. Una vez situados en el directorio que contiene [http://opencv.org/ OpenCV] vamos a ejecutar los siguientes comandos en un terminal:

<syntaxhighlight enclose="div">
mkdir build
cd build
cmake -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON ..
make
sudo make install
</syntaxhighlight>

Solo queda configurar [http://opencv.org/ OpenCV]. Ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight>sudo gedit /etc/ld.so.conf.d/opencv.conf</syntaxhighlight>

Añadimos la siguiente línea de código al archivo que hemos abierto y guardamos los cambios:

<syntaxhighlight>/usr/local/lib</syntaxhighlight>

Ahora ejecutamos en un terminal los siguientes comandos:

<syntaxhighlight>
sudo ldconfig
sudo gedit /etc/bash.bashrc 
</syntaxhighlight>

En el archivo que hemos abierto añadimos las siguientes líneas al final y guardamos los cambios:

<syntaxhighlight>
PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig
export PKG_CONFIG_PATH
</syntaxhighlight>

Para finalizar la instalación vamos a reiniciar el equipo.

===Modificación de cv_bridge===

Debemos modificar el archivo "manifest.xml" del ''package'' [http://wiki.ros.org/cv_bridge?distro=electric cv_bridge] para que no utilice anterior versión de [http://opencv.org/ OpenCV]. Ejecutaremos en un terminal la siguiente secuencia de comandos:

<syntaxhighlight>
roscd cv_bridge
sudo gedit manifest.xml
</syntaxhighlight>

Eliminaremos o comentaremos, para poder reutilizarlas, las siguientes líneas del archivo que hemos abierto:

<syntaxhighlight lang="xml">
<depend package="opencv2" />
...
<rosdep name="opencv2.3"/>
</syntaxhighlight>

===Modificación del ''package''===

Debemos indicar al ''package'' que libreria [http://opencv.org/ OpenCV] debe emplear para compilar. Lo primero debemos asegurarnos que no existe ningún tipo de dependencia en el archivo "manifest.xml" del ''package'' a los ''packages'' del ''stack'' [http://wiki.ros.org/vision_opencv vision_opencv], excepto [http://wiki.ros.org/cv_bridge?distro=electric cv_bridge] que anteriormente ha sido modificado para poder emplearlo. En un terminal ejecutaremos la siguiente secuencia de comandos:

<syntaxhighlight>
roscd find_object_2d
sudo gedit CMakeLists.txt
</syntaxhighlight>

En el archivo que hemos abierto vamos a modificar la siguiente línea:

<syntaxhighlight>find_package(OpenCV REQUIRED)</syntaxhighlight>

y nos quedará asi:
 
<syntaxhighlight>find_package(OpenCV 2.4 REQUIRED)</syntaxhighlight>

En este caso el package ya tenia incluidas las llamadas a librerias [http://opencv.org/ OpenCV] externas a [http://wiki.ros.org/ ROS], si no hubiera sido este el caso tendríamos que añadir las siguientes líneas al archivo "CMakeLists.txt":
 
<syntaxhighlight>
find_package(OpenCV "NÚMERO_VERSIÓN" REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
...
target_link_libraries("NOMBRE_PROGRAMA_A_COMPILAR" ${OpenCV_LIBS})
</syntaxhighlight>

=Cálculo de posición de objetos=

A partir de los datos obtenidos del ''package'' [http://code.google.com/p/find-object/ find_object_2d] se realizan una serie de cálculos para determinar la posición del objeto respecto al [[Control brazo MYRAbot (bioloid+arduino) | brazo del MYRAbot]]. Para esto necesitaremos conocer y establecer una serie de parámetros físicos:

* Posición de la cámara web respecto al brazo (h, d).
* Ángulo de inclinación de la cámara web respecto a la horizontal (&alpha;).
* Distancia perpendicular al objetivo de la cámara a la que vamos a situar los objetos para obtener la imagen de muestra (''dZ0'').

[[file:webcam_MYRAbot_esquema.jpg|thumb|500px|center|Esquema cálculo posición objetos]]

Para el cálculo de la distancia a los objetos se ha tomado el área de estos como base de cálculo, ya que su variación entre las imágenes tomadas es proporcional y lineal respecto a la distancia al objetivo de la cámara. Las ecuaciones obtenidas a partir del esquema son las siguientes, los parámetros se han obtenido realizando mediciones para diferentes posiciones conocidas de un objeto:

[[file:webcam_ecuaciones.jpg]]

==Programa de publicación de posición de objetos==

Lo primero [[Control brazo MYRAbot (bioloid+arduino)#Creación de mensajes personalizados en ROS|crearemos unos mensajes personalizados]] dentro del ''package'' "[http://code.google.com/p/find-object/ find_object_2d]" para la publicación de las posiciones de los objetos en la escena. Como en la escena puede haber más de un objeto reconocido debemos guardar los datos en un array, de dimensión el número de objetos encontrados. Comenzaremos creando un mensaje base llamado "Point_id.msg" con el siguiente contenido, el identificador del objeto y el punto donde se encuentra:

<syntaxhighlight>int16 id
geometry_msgs/Point punto</syntaxhighlight>

Ahora necesitamos crear un mensaje que contenga un array de los mensajes anteriores. Este mensaje lo llamaremos "PointObjects.msg" con el siguiente contenido:

<syntaxhighlight>Point_id[] objeto</syntaxhighlight>

El programa crea el ''nodo'' "objects_detected" que está suscrito al ''topic'' "objects", publicado por el programa "find_object_2d_node", y publica el ''topic'' "point", con el identificador y posición de los objetos encontrados en la escena. Crearemos en este mismo ''package'' un archivo llamado "objects_detected.cpp" en el directorio "src" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <QTransform>
#include <geometry_msgs/Point.h>
#include <std_msgs/Int16.h>
#include <find_object_2d/PointObjects.h>
#include <find_object_2d/Point_id.h>

#define dZ0 450
#define alfa 40
#define h 310
#define d 50
#define PI 3.14159265


void objectsDetectedCallback(const std_msgs::Float32MultiArray& msg)
{
    ros::NodeHandle nh;
    ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);
   
    find_object_2d::PointObjects p_objects;
    find_object_2d::Point_id objeto;
           
    p_objects.objeto = std::vector<find_object_2d::Point_id>(msg.data.size()/12);
           
    for(unsigned int i=0; i<msg.data.size(); i+=12)
    {
        // get data
        int id = (int)msg.data[i];
        float objectWidth = msg.data[i+1];
        float objectHeight = msg.data[i+2];

        // Find corners Qt
        QTransform qtHomography(msg.data[i+3], msg.data[i+4], msg.data[i+5],
                                msg.data[i+6], msg.data[i+7], msg.data[i+8],
                                msg.data[i+9], msg.data[i+10], msg.data[i+11]);

        QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
        QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
        QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
        QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
       
        geometry_msgs::Point punto;
       
	float widthTop = sqrt(pow(qtTopRight.x() - qtTopLeft.x(),2) + pow(qtTopRight.y() - qtTopLeft.y(),2));
	float widthBottom = sqrt(pow(qtBottomRight.x() - qtBottomLeft.x(),2) + pow(qtBottomRight.y() - qtBottomLeft.y(),2));
	float heightLeft = sqrt(pow(qtBottomLeft.x() - qtTopLeft.x(),2) + pow(qtBottomLeft.y() - qtTopLeft.y(),2));
	float heightRight = sqrt(pow(qtBottomRight.x() - qtTopRight.x(),2) + pow(qtBottomRight.y() - qtTopRight.y(),2));
		
        float dArea_0 = (objectHeight*objectWidth) - (((widthTop + widthBottom)/2) * ((heightLeft + heightRight)/2));

        float dZ_0 = dZ0 + (dArea_0/10);
        
        float dY_0 = (((480/2) - (((qtTopLeft.y() + qtTopRight.y())/2) + ((heightLeft + heightRight)/4)))*dZ_0)/585;                
        
        float beta_0 = atan2(dY_0,dZ_0);        
        
        objectHeight = objectHeight/cos((alfa*PI)/180);
        
        float height = ((heightLeft + heightRight)/2)/cos(((alfa*PI)/180)-beta_0);                    
        
        float dArea = (objectHeight*objectWidth) - (((widthTop + widthBottom)/2) * height);

        float dZ = dZ0 + (dArea/38);
       
        float dX = (((640/2) - (((qtTopLeft.x() + qtBottomLeft.x())/2) + ((widthTop + widthBottom)/4)))*dZ)/585;
       
        float dY = (((480/2) - (((qtTopLeft.y() + qtTopRight.y())/2) + ((heightLeft + heightRight)/4)))*dZ)/585;
       
        float beta = atan2(dY,dZ);
       
        punto.x = dX;
        punto.y = h-((dZ/cos(beta))*sin(((alfa*PI)/180)-beta));
        punto.z = ((dZ/cos(beta))*cos(((alfa*PI)/180)-beta))-d;
        
        //Validate detection
        int paralelepipedo;
        
        if (abs(widthTop - widthBottom) < 20 && abs(heightLeft - heightRight) < 15)
        {
			paralelepipedo = 1;
		}
		else
		{
			paralelepipedo = 0;
		}
        
        if (paralelepipedo == 1)
        {       
			objeto.punto = punto;
			objeto.id = id;
       
			p_objects.objeto[i/12] = objeto;
	}

    }
   
    position_pub_.publish(p_objects);

}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "objects_detected");

    ros::NodeHandle nh;
    ros::Subscriber subs = nh.subscribe("objects", 1, objectsDetectedCallback);

    ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);  

    ros::spin();

    return 0;
}
</syntaxhighlight>

===Compilando el programa===

Para generar el ejecutable de nuestro programa tenemos que compilarlo, para ello tenemos que añadir las siguientes líneas de código al final del archivo "CMakeLists.txt" del ''package'':

<syntaxhighlight>
rosbuild_add_executable(objects_detected src/objects_detected.cpp)
target_link_libraries(objects_detected ${LIBRARIES})
</syntaxhighlight>

Para iniciar la compilación ejecutaremos la siguiente secuencia de comandos en un terminal:

<syntaxhighlight>
roscd find_object_2d
make
</syntaxhighlight>

===Ejecutando el programa===

Para poder lanzar nuestra cámara web necesitamos instalar el ''package'' [http://wiki.ros.org/camera_umd?distro=electric camera_umd] y ejecutar el siguiente comando en un terminal:

<syntaxhighlight>rosrun uvc_camera camera_node</syntaxhighlight>

* '''nota:''' Puede que en nuestro equipo se encuentren conectadas más de una cámara web,y la que queremos emplear no sea el dispositivo por defecto "/dev/video0". Para poder cambiar el dispositivo que vamos a emplear debemos ejecutar el siguiente comando en un terminal, indicando el número de nuestro dispositivo:

<syntaxhighlight>rosparam set uvc_camera/device /dev/videoNÚMERO_DE_NUESTRO_DISPOSITIVO</syntaxhighlight>

Para lanzar la cámara web, ejecutar [http://code.google.com/p/find-object/ find_object_2d] y nuestro programa en un solo paso vamos a crear un ''launcher'' que guardaremos en el directorio "launch" dentro del ''package'' con el nombre "encontrar_objetos.launch". El contenido del ''launch'' será el siguiente:

<syntaxhighlight lang="xml">
<launch>
 
  <node name="camera_node" pkg="uvc_camera" type="camera_node">
    <param name="/device" value="/dev/videoNÚMERO_DE_NUESTRO_DISPOSITIVO"/>
  </node>
 
  <node name="find_object_2d" pkg="find_object_2d" type="find_object_2d">
    <remap from="image" to="image_raw"/>
  </node>
 
  <node name="objects_detected" pkg="find_object_2d" type="objects_detected" />
 
</launch>
</syntaxhighlight>

Para iniciar el ''launcher'' simplemente ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight>roslaunch find_object_2d encontrar_objetos.launch</syntaxhighlight>

=Señalar objetos (Escoja)=

Usando la detección y cálculo de posición de objetos se ha desarrollado un programa para controlar el brazo y señalar el objeto que le indiquemos. Este programa se suscribe a los ''topics'' "point", que publica el programa anteriormente descrito, "selected_object", donde publicamos el identificador del objeto a señalar, y "pose_arm", donde se publican los datos del brazo. Publica en los ''topics'' "move_arm" y "hand_arm", donde se indican los movimientos del brazo. Al iniciar el programa el brazo se situará en su posición inicial y una vez publiquemos el número del identificador del objeto, se desplazará hasta señalarlo y pasado un breve período de tiempo, regresará a la posición de partida. Si indicamos el número del identificador de un objeto que no se encuentra en la escena, nos dará un mensaje de objeto no presente (el valor 100 se ha reservado para devolver el brazo a su posición inicial. Crearemos un archivo llamado "escoja.cpp" en el directorio "src", del ''package'' del [[Control brazo MYRAbot (bioloid+arduino)#Programas de control|brazo]], 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 "find_object_2d/PointObjects.h"
  #include "find_object_2d/Point_id.h"
  #include "std_msgs/Int16.h"
  #include "math.h"
 
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60
 
 
  brazo_fer::Servos move, vel, pg, eg, cg;
  find_object_2d::PointObjects p_objects;
  
  int cont = 0;
  int cont_obj = 0;
  int nuevo_punto = 0;
  
  int id_0 = 0;
  int id_1 = 0;
  
  double x = 0;
  double y = 0;
  double z = 0;
  
 
  brazo_fer::Servos inversa(geometry_msgs::Point destino)
  {
    
   	double z = destino.z; 
        double x = destino.x;
	double y = destino.y;
	
	y = y + z*tan(atan2(45,250)); //Compensación para alcanzar la "y" intruducida, ya que la real es menor debido al peso del conjunto.
 
	int coordenadas_correctas = 1;
 
	double alfa, beta, beta_a, beta_p, gamma, delta, delta_a, delta_p;
	double z_p;
	double L_a, L;
 
	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,2)+pow(z_p-Lp,2));
 
	beta_p = atan2(y,z_p-Lp);
 
	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 (y<0) {
		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 - ::pg.base)/5;
	::vel.arti1 = abs(::move.arti1 - ::pg.arti1)/5;	
	::vel.arti2 = abs(::move.arti2 - ::pg.arti2)/5;
	::vel.arti3 = abs(::move.arti3 - ::pg.arti3)/5;
	::vel.pinza = abs(::move.pinza - ::pg.pinza);
 
 
	if (coordenadas_correctas == 1 && (205 <= ::move.base && ::move.base <= 818) && (120 <= ::move.arti1 && ::move.arti1 <= 920) && ::move.arti2 >= 50  && ::move.arti3 <= 828) {
		return ::move;
	}
	else {
		std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
		
		::nuevo_punto = 0;	
		::id_1 = ::id_0;		
		return ::pg;				
	}  
  }
  
  void indicar(geometry_msgs::Point indicado)
  {
	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 mover;
		
		double x0 = indicado.x;
		double z0 = indicado.z;
		
		indicado.z = indicado.z - 100*cos(atan2(x0,z0));
		indicado.x = indicado.x - 100*sin(atan2(x0,z0)); 
		
		mover = inversa(indicado);
		
		brazo_fer::WriteServos indicar;
		
		indicar.posicion = mover;
		
		indicar.velocidad = ::vel;
		
		indicar.par.base = 1;
		indicar.par.arti1 = 1;
		indicar.par.arti2 = 1;
		indicar.par.arti3 = 1;
		indicar.par.pinza = 1;		
						
		move_pub_.publish(indicar);
		hand_pub_.publish(indicar);
		
  }
  
  void home()
  {
	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 mover;
		
		geometry_msgs::Point home;
		
		home.x = 0;
		home.y = 80;
		home.z = 50;
		
		mover = inversa(home);
		
		brazo_fer::WriteServos inicio;
		
		inicio.posicion = mover;
		
		inicio.velocidad = ::vel;
		
		inicio.par.base = 1;
		inicio.par.arti1 = 1;
		inicio.par.arti2 = 1;
		inicio.par.arti3 = 1;
		inicio.par.pinza = 1;		
						
		move_pub_.publish(inicio);
		hand_pub_.publish(inicio);
		
  }
 
  void posicion(const brazo_fer::ReadServos& pose)
  {
 
    brazo_fer::Servos p = pose.posicion;
	
    ::pg = pose.posicion;
    ::eg = pose.estado;
    ::cg = pose.corriente;
    
    if (::cont == 0) 
    {
		home();
		::cont = 1;
	}
	else if (::nuevo_punto > 8)
	{
		if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15)))
		{
			ros::Time ahora;
			
			ahora = ros::Time::now();   //Asignación del tiempo actual a la variable ahora
 
			while (ros::Time::now() < (ahora + ros::Duration(1)))
			{}
			
			home();
			
			::nuevo_punto = 0;	
			::id_1 = ::id_0;
		}
	}
 
  }
 
  void punto(const find_object_2d::PointObjects& point)
  {	
	::p_objects = point;
	
	
	if (::id_0 == ::id_1)
	{
		::cont_obj = 0;
			  
		::x = 0;
		::y = 0;
		::z = 0; 
	}
	else
	{
		if (::id_1 == 100)
		{	
			home();
			::nuevo_punto = 0;		
			::id_1 = ::id_0;
		} 
		else
		{
			int max = ::p_objects.objeto.size();
			
			if (::nuevo_punto < 8)
			{
				for (int i=0; i < max; i++)
				{
					if (::p_objects.objeto[i].id == ::id_1)
					{
						::x = ::x + ::p_objects.objeto[i].punto.x;
						::y = ::y + ::p_objects.objeto[i].punto.y;
						::z = ::z + ::p_objects.objeto[i].punto.z;
										
						::cont_obj = ::cont_obj + 1;
					}
				}
				::nuevo_punto = ::nuevo_punto + 1;			
			}	
			else
			{		
				if (::cont_obj == 0)
				{
					std::cout<<"Objeto no presente en la escena"<<std::endl;
					::nuevo_punto = 0;
				}
				else
				{
					geometry_msgs::Point punto_medio;
				
					punto_medio.x = ::x/::cont_obj;
					punto_medio.y = ::y/::cont_obj;
					punto_medio.z = ::z/::cont_obj;
			
					indicar(punto_medio);
					
					::nuevo_punto = ::nuevo_punto + 1;										
				}
				::id_1 = ::id_0;
			}
		}
	}

  } 
 
  void objeto(const std_msgs::Int16& id)
  {
	::id_1 = id.data;
  }
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "indicar_objetos");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
 
  	ros::Subscriber point_sub_= n.subscribe("point", 1, punto);
  	
  	ros::Subscriber object_sub_= n.subscribe("selected_object", 1, objeto);  	 
 
  	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>

Una vez tengamos [[Fernando-TFM-ROS02#Escribiendo y compilando el programa|compilado el programa]], vamos a crear un ''launcher'' para su ejecución, en el que iniciaremos el nodo de comunicación con [http://www.arduino.cc/es/ arduino], el programa creado e incluiremos el ''launcher'' anteriormente creado, que inicia la cámara web, [http://code.google.com/p/find-object/ find_object_2d] y el programa "objects_detected". Crearemos un archivo llamado "escoja.launch" en el directorio "launch" del ''package'' del [[Control brazo MYRAbot (bioloid+arduino)#Programas de control|brazo]] con el siguiente contenido:

<syntaxhighlight lang="xml">

<launch>

  <node name="serial_node" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0" />
  
  <node name="indicar" pkg="brazo_fer" type="escoja" />

  <include file="$(find find_object_2d)/launch/encontrar_objetos.launch"/>

</launch>

</syntaxhighlight>

Para iniciar el ''launcher'' simplemente ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>
En la sección "[[Modelo para simulación MYRAbot (urdf+gazebo)#Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja|Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja]]" del apartado dedicado al [[Modelo para simulación MYRAbot (urdf+gazebo)|modelo para simulación MYRAbot]], puede verse un vídeo de la ejecución del programa en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo].

=Seguir objetos=

El siguiente programa controla el [[Control brazo MYRAbot (bioloid+arduino)|brazo]] para realizar el seguimiento del objeto que indiquemos a través del ''topic'' "selected_object", haciendo uso del ''package'' [http://code.google.com/p/find-object/ find_object_2d] y el programa anterior para la detección de objetos. Crearemos un archivo llamado "seguir_objetos.cpp" dentro del directorio "src" del ''package'' del [[Control brazo MYRAbot (bioloid+arduino)#Programas de control|brazo]], 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 "find_object_2d/PointObjects.h"
  #include "find_object_2d/Point_id.h"
  #include "std_msgs/Int16.h"
  #include "math.h"
 
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60
 
 
  brazo_fer::Servos move_0, move_1, vel, pg, eg, cg;
  find_object_2d::PointObjects p_objects;
  geometry_msgs::Point punto_obj_0;
  
  int cont = 0;
  int cont_obj = 0;
  
  int id;
  
 
  brazo_fer::Servos inversa(geometry_msgs::Point destino)
  {
    
   	double z = destino.z; 
    double x = destino.x;
	double y = destino.y;
	
	y = y + z*tan(atan2(45,250)); //Compensación para alcanzar la "y" intruducida, ya que la real es menor debido al peso del conjunto.
 
	int coordenadas_correctas = 1;
 
	double alfa, beta, beta_a, beta_p, gamma, delta, delta_a, delta_p;
	double z_p;
	double L_a, L;
 
	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,2)+pow(z_p-Lp,2));
 
	beta_p = atan2(y,z_p-Lp);
 
	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 (y<0) {
		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_1.base = ((alfa+150)*1023)/300;
	::move_1.arti1 = ((beta+60)*1023)/300;
	::move_1.arti2 = ((gamma-30)*1023)/300;
	::move_1.arti3 = ((delta-30)*1023)/300;
	::move_1.pinza = 511;
	
	::vel.base = abs(::move_1.base - ::pg.base)/5;
	::vel.arti1 = abs(::move_1.arti1 - ::pg.arti1)/5;	
	::vel.arti2 = abs(::move_1.arti2 - ::pg.arti2)/5;
	::vel.arti3 = abs(::move_1.arti3 - ::pg.arti3)/5;
	::vel.pinza = abs(::move_1.pinza - ::pg.pinza);		
 
 
	if (coordenadas_correctas == 1 && (205 <= ::move_1.base && ::move_1.base <= 818) && (120 <= ::move_1.arti1 && ::move_1.arti1 <= 920) && ::move_1.arti2 >= 50  && ::move_1.arti3 <= 828) {
		move_0 = move_1;
		
		return ::move_0;
	}
	else {
		std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
			
		return ::move_0;				
	}  
  }
  
  void indicar(geometry_msgs::Point indicado)
  {
	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 mover;
		
		double x0 = indicado.x;
		double z0 = indicado.z;
		
		indicado.z = indicado.z - 150*cos(atan2(x0,z0));
		indicado.x = indicado.x - 150*sin(atan2(x0,z0)); 
		
		mover = inversa(indicado);
		
		brazo_fer::WriteServos indicar;
		
		indicar.posicion = mover;
		
		indicar.velocidad = ::vel;
		
		indicar.par.base = 1;
		indicar.par.arti1 = 1;
		indicar.par.arti2 = 1;
		indicar.par.arti3 = 1;
		indicar.par.pinza = 1;		
						
		move_pub_.publish(indicar);
		hand_pub_.publish(indicar);
		
  }
  
  void home()
  {
	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 mover;
		
		geometry_msgs::Point home;
		
		home.x = 0;
		home.y = 80;
		home.z = 50;
		
		mover = inversa(home);
		
		brazo_fer::WriteServos inicio;
		
		inicio.posicion = mover;
		
		inicio.velocidad = ::vel;
		
		inicio.par.base = 1;
		inicio.par.arti1 = 1;
		inicio.par.arti2 = 1;
		inicio.par.arti3 = 1;
		inicio.par.pinza = 1;		
						
		move_pub_.publish(inicio);
		hand_pub_.publish(inicio);
		
  }
 
  void posicion(const brazo_fer::ReadServos& pose)
  {
 
	brazo_fer::Servos p = pose.posicion;
	
	::pg = pose.posicion;
        ::eg = pose.estado;
        ::cg = pose.corriente;
 
  }
 
  void punto(const find_object_2d::PointObjects& point)
  {	
	::p_objects = point;
	
	geometry_msgs::Point punto_obj_1;
	
	
	if (::id == 100 || ::cont == 0)
	{	
		home();
	
		::cont = ::cont + 1;
	} 

	int max = ::p_objects.objeto.size();

	for (int i=0; i < max; i++)
	{
		if (::p_objects.objeto[i].id == ::id)
		{
			punto_obj_1 = ::p_objects.objeto[i].punto;
			
			::cont_obj = 1;

		}
	}
	
	if (::cont_obj == 0)
	{
		std::cout<<"Objeto no presente en la escena"<<std::endl;

	}
	else if (abs(punto_obj_1.x - ::punto_obj_0.x) > 20 || abs(punto_obj_1.y - ::punto_obj_0.y) > 20 || abs(punto_obj_1.z - ::punto_obj_0.z) > 20)
	{
		indicar(punto_obj_1);
		
		::punto_obj_0 = punto_obj_1;									
	}
	
	::cont_obj = 0;

  } 
 
  void objeto(const std_msgs::Int16& id)
  {
	::id = id.data;
  }
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "seguir_objetos");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
 
  	ros::Subscriber point_sub_= n.subscribe("point", 1, punto);
  	
  	ros::Subscriber object_sub_= n.subscribe("selected_object", 1, objeto);  	 
 
  	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>

Una vez tengamos [[Fernando-TFM-ROS02#Escribiendo y compilando el programa|compilado el programa]], vamos a crear un ''launcher'' para su ejecución, en el que iniciaremos el nodo de comunicación con [http://www.arduino.cc/es/ arduino], el programa creado e incluiremos el ''launcher'' anteriormente creado, que inicia la cámara web, [http://code.google.com/p/find-object/ find_object_2d] y el programa "objects_detected". Crearemos un archivo llamado "seguir.launch" en el directorio "launch" del ''package'' del [[Control brazo MYRAbot (bioloid+arduino)#Programas de control|brazo]] con el siguiente contenido:

<syntaxhighlight lang="xml">

<launch>

  <node name="serial_node" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0" />
  
  <node name="seguir" pkg="brazo_fer" type="seguir_objetos" />

  <include file="$(find find_object_2d)/launch/encontrar_objetos.launch"/>

</launch>

</syntaxhighlight>

Para iniciar el ''launcher'' simplemente ejecutaremos en un terminal el siguiente comando:

<syntaxhighlight>roslaunch brazo_fer seguir.launch</syntaxhighlight>

En la sección "[[Modelo para simulación MYRAbot (urdf+gazebo)#Ejecución del programa seguir objetos|Ejecución del programa seguir objetos]]" del apartado dedicado al [[Modelo para simulación MYRAbot (urdf+gazebo)|modelo para simulación MYRAbot]], puede verse un vídeo de la ejecución del programa en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo].


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

[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]

| valign="top" |

[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
[[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]]