View source for Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

[[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)]]<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!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
[[Detección y cálculo de posición de objetos (cámara web)]]<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)]]
----
[[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)]]<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!)]]

| valign="top" |

[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
[[Detección y cálculo de posición de objetos (cámara web)]]<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)]]
----
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]

|}
----


[[Mobile manipulation | < volver a principal]]