Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)
Contents
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:
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 "Base_Control" y "Group_Motor_Control" para el control de la base y el 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:
catkin_create_pkg cervantes_teleop cervantes_epos_manager geometry_msgs sensor_msgs tf tf_conversions std_msgs moveit_core moveit_ros_perception moveit_ros_planning_interface ros_cppCrearemos un archivo llamado "teleop_xbox360.cpp" dentro del directorio "src" del package creado con el siguiente contenido:
#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;
}
Para compilarlo y generar el ejecutable se debe añadir las siguientes líneas 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:
add_executable(teleop_xbox360 src/teleop_xbox360.cpp)
target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)
Para compilar el programa hay que situarse en el directorio raiz de nuestro espacio de trabajo catkin. Simplemente con ejecutar el siguiente comando en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
catkin_makeCrearemos un archivo llamado "teleop_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:
<launch>
<include file="$(find cervantes_epos_manager)/launch/cervantes_bringup.launch" />
<node pkg="joy" type="joy_node" name="joystick"/>
<node name="teleop_xbox360" pkg="cervantes_teleop" type="teleop_xbox360" output="screen" />
</launch>
Para iniciar el teleoperador simplemente deberemos ejecutar en un terminal el siguiente comando:
roslaunch cervantes_teleop teleop_xbox360.launchEn el siguiente vídeo puede verse el uso del teleoperador para CeRVaNTeS en el simulador:
