| 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:
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.
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.
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 }
