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
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 "Base_Control" and "Group_Motor_Control" for the control of the base and the arm. We have to execute the next command in a terminal in order to create a new package named "cervantes_teleop" within our catkin workspace:
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_cppWe will create a new file named "teleop_xbox360.cpp" within the folder "src" of the created package with the content that is shown below:
#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;
}
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:
add_executable(teleop_xbox360 src/teleop_xbox360.cpp)
target_link_libraries(teleop_xbox360
${catkin_LIBRARIES}
)
We will execute the next command in a terminal, placed in the root folder of the our catkin work space, in order to compile and generate the executable:
roscd myrabot_teleop
makeWe will create a file named "teleop_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>
<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>
We will execute the next command in a terminal in order to start the teleoperator:
roslaunch cervantes_teleop teleop_xbox360.launchIn the next video we can see de teleoperation of CeRVaNTeS in the gazebo simulator:
