[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials.
(!) 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.

How to Write a Generic Teleoperation Node

Description: This tutorial goes over generic teleoperation code that is used as an example in many of the teleoperation tutorials.

Tutorial Level: BEGINNER

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 

   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.


2019-09-14 12:51