Note: This tutorial assumes that you have completed the previous tutorials: Configuring and Using a Linux-Supported Joystick with ROS.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a Teleoperation Node for a Linux-Supported Joystick

Description: This tutorial covers how to write a teleoperation node and use it to drive the turtle in the turtlesim.

Keywords: teleoperation, joystick

Tutorial Level: BEGINNER

This tutorial is a getting started exercise. If you are simply looking for a generic teleop node to use with your Twist robot, consider teleop_twist_joy.

Create a catkin Package

In your catkin workspace:

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_joy roscpp turtlesim joy
$ cd ~/catkin_ws/
$ catkin_make

Writing a Simple Teleoperation Node

First, create learning_joy/src/turtle_teleop_joy.cpp in your favorite editor, and place the following code inside of it.

NOTE: Example code using Python and ROS Hydro can be found here: http://andrewdai.co/xbox-controller-ros.html#rosjoy

   1 #include <ros/ros.h>
   2 #include <geometry_msgs/Twist.h>
   3 #include <sensor_msgs/Joy.h>
   4 
   5 
   6 class TeleopTurtle
   7 {
   8 public:
   9   TeleopTurtle();
  10 
  11 private:
  12   void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  13 
  14   ros::NodeHandle nh_;
  15 
  16   int linear_, angular_;
  17   double l_scale_, a_scale_;
  18   ros::Publisher vel_pub_;
  19   ros::Subscriber joy_sub_;
  20 
  21 };
  22 
  23 
  24 TeleopTurtle::TeleopTurtle():
  25   linear_(1),
  26   angular_(2)
  27 {
  28 
  29   nh_.param("axis_linear", linear_, linear_);
  30   nh_.param("axis_angular", angular_, angular_);
  31   nh_.param("scale_angular", a_scale_, a_scale_);
  32   nh_.param("scale_linear", l_scale_, l_scale_);
  33 
  34 
  35   vel_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
  36 
  37 
  38   joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::joyCallback, this);
  39 
  40 }
  41 
  42 void TeleopTurtle::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
  43 {
  44   geometry_msgs::Twist twist;
  45   twist.angular.z = a_scale_*joy->axes[angular_];
  46   twist.linear.x = l_scale_*joy->axes[linear_];
  47   vel_pub_.publish(twist);
  48 }
  49 
  50 
  51 int main(int argc, char** argv)
  52 {
  53   ros::init(argc, argv, "teleop_turtle");
  54   TeleopTurtle teleop_turtle;
  55 
  56   ros::spin();
  57 }

The Code Explained

Now, let's break down the code piece by piece.

   1 #include <ros/ros.h>
   2 #include <geometry_msgs/Twist.h>
   3 #include <sensor_msgs/Joy.h>
   4 

  • geometry_msgs/Twist.h includes the twist msg so that we can publish twist commands to the turtle
  • sensor_msgs/Joy.h includes the joystick msg so that we can listen to the joy topic

   6 class TeleopTurtle
   7 {
   8 public:
   9   TeleopTurtle();
  10 
  11 private:
  12   void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  13 
  14   ros::NodeHandle nh_;
  15 
  16   int linear_, angular_;
  17   double l_scale_, a_scale_;
  18   ros::Publisher vel_pub_;
  19   ros::Subscriber joy_sub_;
  20 
  21 };

Here we create the TeleopTurtle class and define the joyCallback function that will take a joy msg. We also create a node handle, publisher, and subscriber for later use.

  24 TeleopTurtle::TeleopTurtle():
  25   linear_(1),
  26   angular_(2)
  27 {
  28 
  29   nh_.param("axis_linear", linear_, linear_);
  30   nh_.param("axis_angular", angular_, angular_);
  31   nh_.param("scale_angular", a_scale_, a_scale_);
  32   nh_.param("scale_linear", l_scale_, l_scale_);

Here we initialize some parameters: the linear_ and angular_ variables are used to define which axes of the joystick will control our turtle. We also check the parameter server for new scalar values for driving the turtle.

  35   vel_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
  36 
  37 
  38   joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::joyCallback, this);

Here we create a publisher that will advertise on the command_velocity topic of the turtle.

  38   joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::joyCallback, this);

Here we subscribe to the joystick topic for the input to drive the turtle. If our node is slow in processing incoming messages on the joystick topic, up to 10 messages will be buffered before any are lost.

  42 void TeleopTurtle::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
  43 {
  44   geometry_msgs::Twist twist;
  45   twist.angular.z = a_scale_*joy->axes[angular_];
  46   twist.linear.x = l_scale_*joy->axes[linear_];
  47   vel_pub_.publish(twist);
  48 }

Here we take the data from the joystick and manipulate it by scaling it and using independent axes to control the linear and angular velocities of the turtle. Finally we publish the prepared message.

  51 int main(int argc, char** argv)
  52 {
  53   ros::init(argc, argv, "teleop_turtle");
  54   TeleopTurtle teleop_turtle;
  55 
  56   ros::spin();
  57 }

Lastly we initialize our ROS node, create a teleop_turtle, and spin our node until Ctrl-C is pressed.

Compiling and Running Turtle Teleop

Add the following lines to your CMakeLists.txt file:

add_executable(turtle_teleop_joy src/turtle_teleop_joy.cpp)
target_link_libraries(turtle_teleop_joy ${catkin_LIBRARIES})

Setting up the Joystick

Before we can run the joystick and teleop together, we need to make the joystick accessible. Connect your joystick as we did in the previous tutorial.

Now list the permissions of the joystick:

$ ls -l /dev/input/jsX

You will see something similar to:

  • crw-rw-XX- 1 root dialout 188, 0 2009-08-14 12:04 /dev/input/js0

If XX is rw: the js device is configured properly.

If XX is --: the js device is not configured properly and you need to:

$ sudo chmod a+rw /dev/input/jsX

Button and Axis mappings

In order to drive the turtle correctly, you'll need to configure the button and axis mappings of the joystick to the correct axes of the teleop node. If you're using a PS3 joystick, see the PS3 Joystick page for the axis mappings. If not, you'll have to check the button mappings manually.

To determine which button on the joystick publishes to each button in the ROS Joy message, view your joystick using

$ roscore

$ rosparam set joy_node/dev "/dev/input/jsX"
$ rosrun joy joy_node

$ rostopic echo joy

By pressing buttons and moving all the sticks, you'll be able to determine the correct mapping.

Writing a Launch File to Start all of the Nodes

Now let's make a launch file to start all of the nodes we need. Create the file launch/turtle_joy.launch and paste the following into it. Make sure you change the "dev/input/jsX" to the correct input.

<launch>
 <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

 <!-- joy node -->
  <node respawn="true" pkg="joy"
        type="joy_node" name="turtle_joy" >
    <param name="dev" type="string" value="/dev/input/jsX" />
    <param name="deadzone" value="0.12" />
  </node>

 <!-- Axes -->
  <param name="axis_linear" value="1" type="int"/>
  <param name="axis_angular" value="0" type="int"/>
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
  <node pkg="learning_joy" type="turtle_teleop_joy" name="teleop"/>
</launch>

Now you can start the nodes to drive your turtle around:

roslaunch learning_joy turtle_joy.launch


CategoryHomepage

Wiki: joy/Tutorials/WritingTeleopNode (last edited 2016-11-20 07:57:10 by MatthewWilson)