|
|
| (12 intermediate revisions by 2 users not shown) |
| Line 1: |
Line 1: |
| − | [[Mobile manipulation | < volver a principal]]
| |
| | | | |
| − | ==Teleoperación de MYRAbot==
| |
| − |
| |
| − | 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 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 MYRAbot:
| |
| − |
| |
| − | [[file:controlador_xbox360_MYRAbot.jpg|thumb|center|800px|Funcionalidad [http://www.microsoft.com/hardware/es-xl/p/xbox-360-wireless-controller-for-windows controlador xbox360] para MYRAbot.]]
| |
| − |
| |
| − | ===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'' "cmd_vel", "move_arm" y "hand_arm" para el control de [http://www.irobot.com/global/es/store/Roomba.aspx?gclid=CMfZy73s570CFTMetAodW3UA7A roomba] y el [[Control brazo MYRAbot (bioloid+arduino)|brazo]]. Comenzaremos creando un nuevo ''package'' en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo y ejecutaremos en un terminal el siguiente comando:
| |
| − |
| |
| − | <syntaxhighlight>roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cpp</syntaxhighlight>
| |
| − |
| |
| − | Crearemos un archivo llamado "teleoperador_xbox360.cpp" dentro del directorio "src" del ''package'' creado 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 "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_b::Servos p, e, c, pinza;
| |
| − | geometry_msgs::Point punto;
| |
| − | int pinza_incli = 0;
| |
| − | int paso = 5;
| |
| − | int cont = 0;
| |
| − |
| |
| − | myrabot_arm_base_b::WriteServos teleop;
| |
| − | myrabot_arm_base_b::WriteServos teleop_pinza;
| |
| − |
| |
| − | float avance = 0.2, giro = 0.5;
| |
| − | int velocidad = 50;
| |
| − | int start = 0;
| |
| − |
| |
| − | 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_b::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_b::WriteServos>("move_arm", 1);
| |
| − |
| |
| − | ros::Publisher hand_pub_=n.advertise<myrabot_arm_base_b::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_b::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_x != 0 || ::boton_y != 0 || ::boton_izquierda != 0 || ::boton_derecha != 0)
| |
| − | {
| |
| − |
| |
| − | if (::boton_start != 0)
| |
| − | {
| |
| − | 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)
| |
| − | {
| |
| − | ::punto.x = ::punto.x + (::pad_derecha_x * ::paso);
| |
| − | ::teleop = inversa(::punto, ::pinza_incli, ::p, ::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 - (::pad_derecha_x * ::paso);}
| |
| − | }
| |
| − |
| |
| − | if (::pad_derecha_y > 0.2 || ::pad_derecha_y < -0.2)
| |
| − | {
| |
| − | ::punto.y = ::punto.y + (::pad_derecha_y * ::paso);
| |
| − | ::teleop = inversa(::punto, ::pinza_incli, ::p, ::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 - (::pad_derecha_y * ::paso);}
| |
| − | }
| |
| − |
| |
| − | if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
| |
| − | {
| |
| − | ::punto.z = ::punto.z + (::gatillo_izquierda * (::paso/2));
| |
| − | ::teleop = inversa(::punto, ::pinza_incli, ::p, ::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 - (::gatillo_izquierda * (::paso/2));}
| |
| − | }
| |
| − |
| |
| − | if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
| |
| − | {
| |
| − | ::punto.z = ::punto.z - (::gatillo_derecha * (::paso/2));
| |
| − | ::teleop = inversa(::punto, ::pinza_incli, ::p, ::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 + (::gatillo_derecha * (::paso/2));}
| |
| − | }
| |
| − |
| |
| − | if (::cruz_arriba != 0)
| |
| − | {
| |
| − | ::pinza_incli = ::pinza_incli - (::paso * ::cruz_arriba);
| |
| − | teleop = inversa(::punto, ::pinza_incli, ::p, ::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);}
| |
| − | }
| |
| − |
| |
| − | if (::cruz_abajo != 0)
| |
| − | {
| |
| − | ::pinza_incli = ::pinza_incli + (::paso * ::cruz_abajo);
| |
| − | teleop = inversa(::punto, ::pinza_incli, ::p, ::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);}
| |
| − | }
| |
| − |
| |
| − | if (::boton_izquierda != 0)
| |
| − | {
| |
| − | ::pinza.pinza = ::pinza.pinza - (::paso * ::boton_izquierda);
| |
| − | ::teleop_pinza = control_pinza(::pinza, ::p, ::c);
| |
| − | if (::teleop_pinza.posicion.pinza == pose.pinza)
| |
| − | {::pinza.pinza = ::pinza.pinza + (::paso * ::boton_izquierda);}
| |
| − | }
| |
| − |
| |
| − | if (::boton_derecha != 0)
| |
| − | {
| |
| − | ::pinza.pinza = ::pinza.pinza + (::paso * ::boton_derecha);
| |
| − | ::teleop_pinza = control_pinza(::pinza, ::p, ::c);
| |
| − | if (::teleop_pinza.posicion.pinza == pose.pinza)
| |
| − | {::pinza.pinza = ::pinza.pinza - (::paso * ::boton_derecha);}
| |
| − | }
| |
| − |
| |
| − | if (::teleop.posicion.base != pose.base || ::teleop.posicion.arti1 != pose.arti1 || ::teleop.posicion.arti2 != pose.arti2 || ::teleop.posicion.arti3 != pose.arti3)
| |
| − | {
| |
| − | move_pub_.publish(::teleop);
| |
| − | }
| |
| − | if (::teleop_pinza.posicion.pinza != pose.pinza)
| |
| − | {
| |
| − | 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;
| |
| − | }
| |
| − | 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;
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | ::base.angular.z = 0;
| |
| − | }
| |
| − |
| |
| − | base_pub_.publish(::base);
| |
| − | }
| |
| − | }
| |
| − |
| |
| − | //std::cout<<::punto<<std::endl;
| |
| − |
| |
| − | ros::spinOnce();
| |
| − |
| |
| − | loop_rate.sleep();
| |
| − | }
| |
| − |
| |
| − | return 0;
| |
| − | }
| |
| − |
| |
| − | </syntaxhighlight>
| |
| − |
| |
| − | Para compilarlo y generar el ejecutable se debe añadir la siguiente línea 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>rosbuild_add_executable(teleoperador_xbox360 src/teleoperador_xbox360.cpp)</syntaxhighlight>
| |
| − |
| |
| − | Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
| |
| − |
| |
| − | <syntaxhighlight>roscd myrabot_teleop
| |
| − | make</syntaxhighlight>
| |
| − |
| |
| − | Crearemos un archivo llamado "teleoperador_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>
| |
| − |
| |
| − | <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>
| |
| − |
| |
| − | Para iniciar el teleoperador simplemente deberemos ejecutar en un terminal el siguiente comando:
| |
| − |
| |
| − | <syntaxhighlight>roslaunch myrabot_teleop teleoperador_xbox360.launch</syntaxhighlight>
| |
| − |
| |
| − | [[Mobile manipulation | < volver a principal]]
| |
