| Note: This tutorial assumes that you have completed the previous tutorials: Configuring and Using a Linux-Supported Joystick with ROS. |
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
Contents
Create a Scratch Package
Before starting this tutorial, take the time to create a scratch package to work in and manipulate the example code. See creating a ROS package to learn more about creating a package. Create a sandbox package with the following dependencies:
$ roscreate-pkg learning_joy roscpp turtlesim joy
Uncomment the genmsg() line in the CMakeLists.txt of the learning_joy package and run rosmake.
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: If you installed joy as a diamondback stack, replace the references to "sensor_msgs" with "joy" throughout this code. See http://answers.ros.org/question/10787/joy-node-build-bug-when-building-from-source for more details.
The Code
1 #include <ros/ros.h>
2 #include <turtlesim/Velocity.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<turtlesim::Velocity>("turtle1/command_velocity", 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 turtlesim::Velocity vel;
45 vel.angular = a_scale_*joy->axes[angular_];
46 vel.linear = l_scale_*joy->axes[linear_];
47 vel_pub_.publish(vel);
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.
- turtlesim/Velocity.h includes the turtle velocity msg so that we can publish velocity commands to the turtle
- joy/Joy.h includes the joystick msg so that we can listen to the joy topic
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.
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.
36 vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
Here we create a publisher that will advertise on the command_velocity topic of the turtle.
39 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.
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.
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 line to your CMakeLists.txt file:
rosbuild_add_executable(turtle_teleop_joy src/turtle_teleop_joy.cpp)
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/js0
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/js0
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
rostopic echo joy
after you launch the joystick. 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:
1 <launch>
2
3 <!-- Turtlesim Node-->
4 <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
5
6
7 <!-- joy node -->
8 <node respawn="true" pkg="joy"
9 type="joy" name="turtle_joy" >
10 <param name="dev" type="string" value="/dev/input/js0" />
11 <param name="deadzone" value="0.12" />
12 </node>
13
14 <!-- Axes -->
15 <param name="axis_linear" value="1" type="int"/>
16 <param name="axis_angular" value="0" type="int"/>
17 <param name="scale_linear" value="2" type="double"/>
18 <param name="scale_angular" value="2" type="double"/>
19
20 <node pkg="turtle_teleop" type="turtle_teleop_joy" name="teleop"/>
21
22 </launch>
Since we built everything in package learning_joy, you may need to change the package in the last line from turtle_teleop to learning_joy.
Now you can start the nodes to drive your turtle around:
roslaunch learning_joy turtle_joy.launch