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 400: Line 400:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 +
<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>
  
  
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]

Revision as of 06:57, 24 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



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