MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)
From robotica.unileon.es
Contents
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 ssh connection between the robot PC and our PC, using the keyboard. This method use a 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 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 xbox360 controller for teleoperation of MYRAbot is shown in the next picture:
Funcionalidad 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 roomba and the arm. We have to execute the next command in a terminal in order to create a new package named "myrabot_teleop" within our workspace:
roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cppWe will create a new file named "teleoperador_xbox360.cpp" within the folder "src" of the created package with the content that is shown below:
#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;
}
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:
rosbuild_add_executable(teleoperador_xbox360 src/teleoperador_xbox360.cpp)
We will execute the next commands an a terminal in order to compile and generate the executable:
roscd myrabot_teleop
makeWe 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:
<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>
We will execute the next command in a terminal in order to start the teleoperator:
roslaunch myrabot_teleop teleoperador_xbox360.launch
