| Note: This tutorial assumes that you have completed the previous tutorials: Pairing and Using a Wiimote 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 the Wiimote
Description: This tutorial covers how to write a teleoperation node and use it to drive the turtle in the turtlesim.Keywords: teleoperation, joystick, Wiimote
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. In the sandbox folder, create a package called learning_wiimote, with dependencies on roscpp, turtlesim and wiimote:
$ roscd sandbox $ roscreate-pkg learning_wiimote roscpp turtlesim wiimote
Uncomment the rosbuild_genmsg() line in the CMakeLists.txt file of the learning_wiimote package and run rosmake.
Writing a Simple Teleoperation Node
First, create learning_wiimote/src/turtle_teleop_wiimote.cpp in your favorite editor, and place the following inside it:
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_wiimote src/turtle_teleop_wiimote.cpp)
Run rosmake on your package.
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 to start: create the file launch/turtle_wii.launch and paste the following into it:
Could not fetch external code from 'https://code.ros.org/svn/ros-pkg/stacks/joystick_drivers_tutorials/trunk/turtle_teleop/launch/turtle_wii.launch':
change the line before the last one to: <node pkg="learning_wiimote" type="turtle_teleop_wiimote" name="teleop"/>
Now you can start your nodes to drive your turtle around:
roslaunch learning_wiimote turtle_wii.launch
Press the 1 & 2 buttons as directed. Now use the move the wiimote controller to drive.
