CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)
From robotica.unileon.es
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 CeRVaNTeS is shown in the next picture:
Funcionalidad xbox360 controller for CeRVaNTeS.
Program
#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;
}
