View source for MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

[[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)]]<br/>
[[Objects recognition and position calculation (webcam)]]<br/>
[[MYRAbot's arm model for simulation (urdf+gazebo)]]<br/>
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
[[Voice control (sphinx+festival)]]

| valign="top" |

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

|}
----


=Teleoperation of MYRAbot=

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 MYRAbot is shown in the next picture:

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

==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'' "cmd_vel", "move_arm" and "hand_arm" for the control of [http://www.irobot.com/global/es/store/Roomba.aspx?gclid=CMfZy73s570CFTMetAodW3UA7A roomba] and the [[MYRAbot's arm control (bioloid+arduino)|arm]]. We have to execute the next command in a terminal in order to create a new ''package'' named "myrabot_teleop" within our workspace:

<syntaxhighlight>roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cpp</syntaxhighlight>

We will create a new file named "teleoperador_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 "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"
  #include "geometry_msgs/Twist.h"
  #include "geometry_msgs/Point.h"    
  #include "sensor_msgs/Joy.h"
  #include "math.h"

myrabot_arm_base::Servos p, e, c, pinza;
geometry_msgs::Point punto;
int pinza_incli = 0;
int paso = 5;
int cont = 0;
	
myrabot_arm_base::WriteServos teleop;
myrabot_arm_base::WriteServos teleop_pinza;

float avance = 0.2, giro = 0.5;
int velocidad = 50;
int start = 0;
int flanco;

geometry_msgs::Twist base;

float 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 posicion_estado_corriente(const myrabot_arm_base::ReadServos& pec)   
  {
	
	::p = pec.posicion;
	::e = pec.estado;
	::c = pec.corriente;
	  
  }	  	 
 
  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 pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);   
  	
  	ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox); 
  	
  	
	ros::Publisher move_pub_=n.advertise<myrabot_arm_base::WriteServos>("move_arm", 1);   

	ros::Publisher hand_pub_=n.advertise<myrabot_arm_base::WriteServos>("hand_arm", 1);
	
	ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); 
		
    ros::Rate loop_rate(5);	
	
	while (ros::ok())
	{
	
		myrabot_arm_base::Servos pose = ::p;	
		
		if (::cont == 0 && (pose.base != 0 && pose.arti1 != 0 && pose.arti2 != 0 && pose.arti3 != 0))
		{
			::punto = home(::p, ::c);	
		
			::cont = 1;
		
			::pinza.pinza = 511;
		
			::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);		
		
			::teleop_pinza = control_pinza(::pinza, ::p, ::c);
		}

		if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2 || ::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2 || ::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_start != 0 || ::boton_a != 0 || ::boton_b != 0 || ::boton_x != 0 || ::boton_y != 0 || ::boton_izquierda != 0 || ::boton_derecha != 0)
		{
		
			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 (::boton_back != 0)
				{
					::cont = 0;
				}
				
				if (::cruz_izquierda != 0)
				{
					::velocidad = ::velocidad - 10;
					
					if (::velocidad < 10)
					{
						::velocidad = 10;
					}
				}
				
				if (::cruz_derecha != 0)
				{
					::velocidad = ::velocidad + 10;
					
					if (::velocidad > 100)
					{
						::velocidad = 100;
					}
				}

				if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
				{
					float f_pad_derecha_x = ::pad_derecha_x;
					::punto.x = ::punto.x + (f_pad_derecha_x * ::paso);
					::teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
					{::punto.x = ::punto.x - (f_pad_derecha_x * ::paso);}
					else
					{move_pub_.publish(::teleop);}
				}
	
				if (pad_derecha_y > 0.2 || pad_derecha_y < -0.2)
				{
					float f_pad_derecha_y = ::pad_derecha_y;
					::punto.y = ::punto.y + (f_pad_derecha_y * ::paso);
					::teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
					{::punto.y = ::punto.y - (f_pad_derecha_y * ::paso);}
					else
					{move_pub_.publish(::teleop);}
				}
				
				if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
				{
					float f_gatillo_izquierda = ::gatillo_izquierda;
					::punto.z = ::punto.z + (f_gatillo_izquierda  * (::paso/2));
					::teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
					{::punto.z = ::punto.z - (f_gatillo_izquierda * (::paso/2));}
					else
					{move_pub_.publish(::teleop);}
				}
				
				if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
				{
					float f_gatillo_derecha = ::gatillo_derecha;
					::punto.z = ::punto.z - (f_gatillo_derecha  * (::paso/2));
					::teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
					{::punto.z = ::punto.z + (f_gatillo_derecha * (::paso/2));}
					else
					{move_pub_.publish(::teleop);}
				}		
				
				if (::cruz_arriba !=  0)
				{
					::pinza_incli = ::pinza_incli - (::paso * ::cruz_arriba);
					teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3) 
					{::pinza_incli = ::pinza_incli + (::paso * ::cruz_arriba);}
					else
					{move_pub_.publish(::teleop);}
				}
				
				if (::cruz_abajo !=  0)
				{
					::pinza_incli = ::pinza_incli + (::paso * ::cruz_abajo);
					::teleop = inversa(::punto, ::pinza_incli, pose, ::velocidad);
					if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3) 
					{::pinza_incli = ::pinza_incli - (::paso * ::cruz_abajo);}
					else
					{move_pub_.publish(::teleop);}
				}
				
				if (::boton_izquierda != 0)
				{     
					::pinza.pinza = ::pinza.pinza - (::paso * ::boton_izquierda);
					::teleop_pinza = control_pinza(::pinza, pose, ::c);
					if (::teleop_pinza.posicion.pinza == pose.pinza) 
					{::pinza.pinza = ::pinza.pinza + (::paso * ::boton_izquierda);}
					else
					{hand_pub_.publish(::teleop_pinza);}	
				}
				
				if (::boton_derecha != 0)
				{     
					::pinza.pinza = ::pinza.pinza + (::paso * ::boton_derecha);
					::teleop_pinza = control_pinza(::pinza, pose, ::c);
					if (::teleop_pinza.posicion.pinza == pose.pinza) 
					{::pinza.pinza = ::pinza.pinza - (::paso * ::boton_derecha);}
					else
					{hand_pub_.publish(::teleop_pinza);}	
				}
				
				if (::boton_a != 0)
				{
					::avance = ::avance - 0.05;
					
					if (::avance < 0.05)
					{
						::avance = 0.05;
					}
				}
				
				if (::boton_b != 0)
				{
					::avance = ::avance + 0.05;
					
					if (::avance > 0.5)
					{
						::avance = 0.5;
					}
				}
				
				if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
				{
					::base.linear.x = ::avance * pad_izquierda_y;
					base_pub_.publish(::base);
				}
				else
				{
					::base.linear.x = 0;
				}
				
				if (::boton_x != 0)
				{
					::giro = ::giro - 0.05;
					
					if (::giro < 0.1)
					{
						::giro = 0.1;
					}
				}
				
				if (::boton_y != 0)
				{
					::giro = ::giro + 0.05;
					
					if (::giro > 1.5)
					{
						::giro = 1.5;
					}
				}
				
				if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
				{
					::base.angular.z = ::giro * pad_izquierda_x;
					base_pub_.publish(::base);
				}
				else
				{
					::base.angular.z = 0;
				}		
			}
		}
		
		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>rosbuild_add_executable(teleoperador_xbox360 src/teleoperador_xbox360.cpp)</syntaxhighlight>

We will execute the next commands an a terminal in order to compile and generate the executable:

<syntaxhighlight>roscd myrabot_teleop
make</syntaxhighlight>

We will create a file named "teleoperador_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>

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

  <node pkg="joy" type="joy_node" name="joystick"/>
  
  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />  
  
  <node name="teleoperador_xbox360" pkg="myrabot_teleop" type="teleoperador_xbox360" output="screen" />

</launch>

</syntaxhighlight>

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

<syntaxhighlight>roslaunch myrabot_teleop teleoperador_xbox360.launch</syntaxhighlight>


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

[[MYRAbot's arm control (bioloid+arduino)]]<br/>
[[Objects recognition and position calculation (webcam)]]<br/>
[[MYRAbot's arm model for simulation (urdf+gazebo)]]<br/>
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
[[Voice control (sphinx+festival)]]

| valign="top" |

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

|}
----


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