Difference between revisions of "Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)"

From robotica.unileon.es
Jump to: navigation, search
(Programa)
Line 31: Line 31:
 
----
 
----
  
 +
 +
=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=
 
=Controlador xbox360=

Revision as of 15:32, 25 September 2014

< volver a principal



Artículos realcionados
Otros artículos

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

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)


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 ssh con el PC del robot y usando el teclado de nuestro PC. Este método emplea una red 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 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 controlador xbox360 para la teleoperación de CeRVaNTeS:

Funcionalidad controlador xbox360 para CeRVaNTeS.

Programa

  #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;
  }
<videoflash>wF_gKr1KWfg#t=13</videoflash>
Ver vídeo en YouTube


< volver a principal