Note: This tutorial assumes that you have completed the previous tutorials: How to Use a SpaceNavigator with the Spacenav_node.
(!) 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 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

Workspace Setup

If you have not yet created a workspace in which to complete the tutorials, click here for some brief instructions .

  Show EOL distros: 

Create a file named ~/tutorials.rosinstall with the following content:

- other: { local-name: workspace }

To overlay on the ROS distro you are using:

rosinstall ~/tutorials /opt/ros/$ROS_DISTRO>> ~/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 to source your script file is to add it 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 catkin workspace like so:

$ source /opt/ros/$ROS_DISTRO/setup.bash
$ mkdir -p ~/tutorial_ws/src
$ cd ~/tutorial_ws
$ catkin_init_workspace src
$ catkin_make

And now source the setup file from the result-space, so that packages you add to this workspace's src folder will be findable by rospack, and the built binaries by rosrun and roslaunch:

$ source devel/setup.bash

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

https://raw.githubusercontent.com/ros-drivers/joystick_drivers_tutorials/master/turtle_teleop/src/teleop_turtle_joy.cpp

   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.

   2 #include <ros/ros.h>
   3 #include <turtlesim/Velocity.h>
   4 #include <sensor_msgs/Joy.h>
   5 

  • 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

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

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.

  25 TeleopTurtle::TeleopTurtle():
  26   linear_(1),
  27   angular_(2)
  28 {
  29 
  30   nh_.param("axis_linear", linear_, linear_);
  31   nh_.param("axis_angular", angular_, angular_);
  32   nh_.param("scale_angular", a_scale_, a_scale_);
  33   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.

  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.

  43 void TeleopTurtle::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
  44 {
  45   turtlesim::Velocity vel;
  46   vel.angular = a_scale_*joy->axes[angular_];
  47   vel.linear = l_scale_*joy->axes[linear_];
  48   vel_pub_.publish(vel);
  49 }

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.

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

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:

Could not fetch external code from 'https://code.ros.org/svn/ros-pkg/stacks/joystick_drivers_tutorials/trunk/turtle_teleop/launch/turtle_spacenav.launch':

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

roslaunch learning_spacenav turtle_spacenav.launch

Wiki: spacenav_node/Tutorials/WritingTeleopNode (last edited 2010-09-16 05:10:09 by MeloneeWise)