CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)
From robotica.unileon.es
Contents
Teleoperation of CeRVaNTeS
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 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;
}
