| Note: This tutorial assumes that you have completed the previous tutorials: How to Use a SpaceNavigator with the Spacenav_node. |
Writing a Teleoperation Node for the SpaceNavigator
Description: This tutorial covers how to write a teleoperation node and use it to drive the turtle in the turtlesim.Keywords: teleoperation, SpaceNavigator
Tutorial Level: BEGINNER
Contents
All these tutorials assume that you have setup your tutorial workspace. If you have not done so click here.
Setting up the ROS tutorial workspace
This describes how to setup your environment for tutorials.
Create a file named ~/tutorials.rosinstall with the following content:
- other: { local-name: workspace }To overlay on cturtle:
rosinstall ~/tutorials /opt/ros/cturtle ~/tutorials.rosinstall
To overlay on boxturtle:
rosinstall ~/tutorials /opt/ros/boxturtle ~/tutorials.rosinstall
To use this workspace whenever you open a new terminal setup your ROS environment by typing:
source ~/tutorials/setup.bash
Sourcing this file adds ~/tutorials/workspace to your ROS_PACKAGE_PATH. Any packages you create in that directory will be found by rospack.
An alternative is to add this to your .bashrc, but remember that this will persist in your .bashrc into the future, and you can only have one environment setup. For more on what this is doing see this page
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 learning_spacenav package with the following dependencies:
$ roscd tutorials $ roscreate-pkg learning_spacenav roscpp turtlesim spacenav_node
Uncomment the genmsg() line in the CMakeLists.txt file of the learning_spacenav package and run rosmake.
Writing a Simple Teleoperation Node
First, create learning_spacenav/src/teleop_turtle_spacenav.cpp in your favorite editor, and place the following inside of 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_spacenav src/turtle_teleop_spacenav.cpp)
Setting up the SpaceNavigator
Before we can run the SpaceNavigator and teleop together, we need to make the SpaceNavigator accessible. Connect your SpaceNavigator as we did in the previous tutorial.
$ roscd /spacenav/spacenav_svn/spacenavd $ sudo ./spacenavd -d
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. See Roslaunch XML Reference for roslaunch syntax. Create the file launch/turtle_spacenav.launch and paste the following:
1 <launch>
2
3 <!-- Turtlesim Node-->
4 <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
5
6
7 <!-- spacenav node -->
8 <node respawn="true" pkg="spacenav_node"
9 type="spacenav_node" name="turtle_spacenav" >
10 <remap from="spacenav/joy" to="joy" />
11 </node>
12
13 <!-- Axes -->
14 <param name="axis_linear" value="1" type="int"/>
15 <param name="axis_angular" value="0" type="int"/>
16 <param name="scale_linear" value="2" type="double"/>
17 <param name="scale_angular" value="2" type="double"/>
18
19 <node pkg="turtle_teleop" type="turtle_teleop_joy" name="teleop"/>
20
21 </launch>
Now you can start the nodes to drive your turtle around:
roslaunch learning_spacenav turtle_spacenav.launch