Note: This tutorial assumes that you have completed the previous tutorials: Using the robot base controllers to drive the robot and have a basic understanding of the Transform Library.
(!) 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.

Using the base controller with odometry and transform information

Description: We move the robot forward by a specified amount by using the low-level base controller together with transform information from odometry.

Keywords: move base, odometry

Tutorial Level: INTERMEDIATE

Overview

In running drive_base in the previous tutorial (Using the robot base controllers to drive the robot), you might have noticed that the robot moves a tiny distance and then stops abruptly after each command, which makes control rather jerky. This is because in order to keep driving, the controller needs to keep receiving commands, or it will assume that something has died and stop the robot.

In this tutorial, we will create a loop that sends velocity commands continuously, until the desired distance has been traveled. We will use the information about the distance traveled so far from the odometry.

Odometry and tf

Recall that in the previous tutorial we checked the base odometry controller along with the base velocity controller. However, we did not use odometry information in any way. We will in this tutorial; but the good news is that we don't have to bother talking to the odometry node directly. The odometry controller just needs to be running; it will then publish all its information into the transform tree which is accessible using the Transform Library tf.

To ensure that both required controllers are running, you can go back to the The base velocity and odometry controller step in the previous tutorial.

Once we have made sure that the odometry controller is running, all that we need to do is ask tf to tell us how the frame of the base of the robot has moved relative to a fixed odometry frame. One of the benefits is that the node we are writing here doesn't need to know where this information is coming from; all that matters is that the tf transform tree has it.

Writing the node

We will start from the node you have created in the previous tutorial (Using the robot base controllers to drive the robot); however, since we will be using tf we must add a dependency on the tf package. Edit the manifest.xml file in your package and add the following dependency:

<depend package="tf" />

We will need to add some functionality to our RobotDriver class. More specifically, we are adding a function for driving forward a certain distance, based on odometry information. At the end of this tutorial you will also find a similar example for turning.

The key code snippets for achieving the desired functionality are:

   1 #include <tf/transform_listener.h>
   2 

We will be using information from tf so we need to include the appropriate header.

  tf::TransformListener listener_;

Our RobotDriver class will use an instance of a tf Listener to receive information from tf.

   1   listener_.waitForTransform("base_footprint", "odom_combined", 
   2                             ros::Time(0), ros::Duration(1.0));

The transform that we are interested in is the one from the base_footprint frame to the odom_combined frame. The odometry node will continuously update this information. Before we do anything, we have to wait for tf to begin receiving this information about this transform from the odometry node, hence the waitForTransform command.

   1   //record the starting transform from the odometry to the base frame
   2   listener_.lookupTransform("base_footprint", "odom_combined", 
   3                             ros::Time(0), start_transform);

The main command for actually receiving the transform information from tf. We will use the same command inside our loop to see when we have travelled the specified distance.

Here is the complete class:

   1 #include <iostream>
   2 
   3 #include <ros/ros.h>
   4 #include <geometry_msgs/Twist.h>
   5 #include <tf/transform_listener.h>
   6 
   7 class RobotDriver
   8 {
   9 private:
  10   //! The node handle we'll be using
  11   ros::NodeHandle nh_;
  12   //! We will be publishing to the "cmd_vel" topic to issue commands
  13   ros::Publisher cmd_vel_pub_;
  14   //! We will be listening to TF transforms as well
  15   tf::TransformListener listener_;
  16 
  17 public:
  18   //! ROS node initialization
  19   RobotDriver(ros::NodeHandle &nh)
  20   {
  21     nh_ = nh;
  22     //set up the publisher for the cmd_vel topic
  23     cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  24   }
  25 
  26   //! Drive forward a specified distance based on odometry information
  27   bool driveForwardOdom(double distance)
  28   {
  29     //wait for the listener to get the first message
  30     listener_.waitForTransform("base_footprint", "odom_combined", 
  31                                ros::Time(0), ros::Duration(1.0));
  32     
  33     //we will record transforms here
  34     tf::StampedTransform start_transform;
  35     tf::StampedTransform current_transform;
  36 
  37     //record the starting transform from the odometry to the base frame
  38     listener_.lookupTransform("base_footprint", "odom_combined", 
  39                               ros::Time(0), start_transform);
  40     
  41     //we will be sending commands of type "twist"
  42     geometry_msgs::Twist base_cmd;
  43     //the command will be to go forward at 0.25 m/s
  44     base_cmd.linear.y = base_cmd.angular.z = 0;
  45     base_cmd.linear.x = 0.25;
  46     
  47     ros::Rate rate(10.0);
  48     bool done = false;
  49     while (!done && nh_.ok())
  50     {
  51       //send the drive command
  52       cmd_vel_pub_.publish(base_cmd);
  53       rate.sleep();
  54       //get the current transform
  55       try
  56       {
  57         listener_.lookupTransform("base_footprint", "odom_combined", 
  58                                   ros::Time(0), current_transform);
  59       }
  60       catch (tf::TransformException ex)
  61       {
  62         ROS_ERROR("%s",ex.what());
  63         break;
  64       }
  65       //see how far we've traveled
  66       tf::Transform relative_transform = 
  67         start_transform.inverse() * current_transform;
  68       double dist_moved = relative_transform.getOrigin().length();
  69 
  70       if(dist_moved > distance) done = true;
  71     }
  72     if (done) return true;
  73     return false;
  74   }
  75 };
  76 
  77 int main(int argc, char** argv)
  78 {
  79   //init the ROS node
  80   ros::init(argc, argv, "robot_driver");
  81   ros::NodeHandle nh;
  82 
  83   RobotDriver driver(nh);
  84   driver.driveForwardOdom(0.5);
  85 }

Compiling and Running

Make sure the following line is in your CMakeLists.txt (it is the same from the previous tutorial):

rosbuild_add_executable(drive_base src/drive_base.cpp)

and make the binary by typing 'make' in the drive_base_tutorial directory.

To run, just as in the previous tutorial, bring up the robot (hardware or simulation), start the controllers (if you have just run the previous tutorial they should still be up and running), and run

bin/drive_base cmd_vel:=base_controller/command

You should see the robot move forward 0.5 meters.

Turning

We could just as easily add a function to our class that turns the robot a specified amount. Note that much of the code is similar to the function for driving forward.

   1   bool turnOdom(bool clockwise, double radians)
   2   {
   3     while(radians < 0) radians += 2*M_PI;
   4     while(radians > 2*M_PI) radians -= 2*M_PI;
   5 
   6     //wait for the listener to get the first message
   7     listener_.waitForTransform("base_footprint", "odom_combined", 
   8                                ros::Time(0), ros::Duration(1.0));
   9     
  10     //we will record transforms here
  11     tf::StampedTransform start_transform;
  12     tf::StampedTransform current_transform;
  13 
  14     //record the starting transform from the odometry to the base frame
  15     listener_.lookupTransform("base_footprint", "odom_combined", 
  16                               ros::Time(0), start_transform);
  17     
  18     //we will be sending commands of type "twist"
  19     geometry_msgs::Twist base_cmd;
  20     //the command will be to turn at 0.75 rad/s
  21     base_cmd.linear.x = base_cmd.linear.y = 0.0;
  22     base_cmd.angular.z = 0.75;
  23     if (clockwise) base_cmd.angular.z = -base_cmd.angular.z;
  24     
  25     //the axis we want to be rotating by
  26     tf::Vector3 desired_turn_axis(0,0,1);
  27     if (!clockwise) desired_turn_axis = -desired_turn_axis;
  28     
  29     ros::Rate rate(10.0);
  30     bool done = false;
  31     while (!done && nh_.ok())
  32     {
  33       //send the drive command
  34       cmd_vel_pub_.publish(base_cmd);
  35       rate.sleep();
  36       //get the current transform
  37       try
  38       {
  39         listener_.lookupTransform("base_footprint", "odom_combined", 
  40                                   ros::Time(0), current_transform);
  41       }
  42       catch (tf::TransformException ex)
  43       {
  44         ROS_ERROR("%s",ex.what());
  45         break;
  46       }
  47       tf::Transform relative_transform = 
  48         start_transform.inverse() * current_transform;
  49       tf::Vector3 actual_turn_axis = 
  50         relative_transform.getRotation().getAxis();
  51       double angle_turned = relative_transform.getRotation().getAngle();
  52       if ( fabs(angle_turned) < 1.0e-2) continue;
  53 
  54       if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) 
  55         angle_turned = 2 * M_PI - angle_turned;
  56 
  57       if (angle_turned > radians) done = true;
  58     }
  59     if (done) return true;
  60     return false;
  61   }

Wiki: pr2_controllers/Tutorials/Using the base controller with odometry and transform information (last edited 2013-01-20 05:02:11 by Furushchev)