Need help to understand tf [from book: ROS by Example]

edit

Hello, reading the book ROS by Example written by Patrick Goebel I m trying to understand and apply some basic concepts regarding tf.

At page 55 he explains an interesting code written using python in order to drive the simulated turtlebot using the transformation /odom and /base_footprint or /odom and /base_link. Since I m really interested to understand ROS very well I tried on my owm to realize a similar program rewriting the whole code in C++. Ok..let's start: I wrote [half] code to move the turtlebot from the origin point (0,0,0) for about 1 meter without turning or making something strange. It just drives the turtlebot straight. Here is my code:

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>

#include <math.h>

class robotMovement 
{

protected:
    ros::NodeHandle nh;
    ros::Publisher turtle_pub;

    tf::TransformListener listener;
    tf::StampedTransform transform;

    std::string odom_frame;
    std::string base_frame;
    std::string str;

    float linear_speed;
    float angular_speed;
    int rate;

public:
    robotMovement( std::string topic, float vel, float rot, int r )
    : str( topic ), linear_speed( vel ), angular_speed( rot ), rate( r )
    {
        this->turtle_pub = nh.advertise<geometry_msgs::Twist>( str, 10 );

        this->odom_frame = "/odom";

        try{
            this->listener.waitForTransform( this->odom_frame, "/base_footprint", ros::Time( 0 ), ros::Duration( 1.0 ) );
            this->base_frame = "/base_footprint";
        }
        catch ( tf::TransformException ex ) {
            ROS_ERROR( "%s", ex.what() );
            ros::Duration( 1.0 ).sleep();

        }

        try{
            this->listener.waitForTransform( this->odom_frame, "/base_link", ros::Time( 0 ), ros::Duration( 1.0 ) );
            this->base_frame = "/base_link";
        }
        catch ( tf::TransformException ex ) {
            ROS_ERROR( "%s", ex.what() );
            ros::Duration( 1.0 ).sleep();

        }
    }

    ~robotMovement( void ){
    }

    void moveStraight( float goal_distance ) 
    {
        float distance = 0.0;
        int counter = 0;

        /* Set the velocity vector */
        geometry_msgs::Twist msg;

        msg.linear.x = linear_speed;
        msg.linear.y = 0.0;
        msg.linear.z = 0.0;

        msg.angular.x = 0.0;
        msg.angular.y = 0.0;
        msg.angular.z = 0.0;


        ros::Rate loop( rate );

        do {

            try{

                listener.lookupTransform( this->odom_frame, this->base_frame, ros::Time( 0 ), transform );

                } catch ( tf::TransformException &ex ) {
                    ROS_ERROR( "%s", ex.what() );
                    ros::Duration( 1.0 ).sleep();
                }

            distance = sqrt( pow( transform.getOrigin().x(), 2 ) + pow( transform.getOrigin().x(), 2 ) );

            robotMovement::turtle_pub.publish( msg );

            ROS_INFO( "[%d], Distance: %6.4f", counter++, distance );

            loop.sleep();

        } while( ros::ok() && ( distance <= goal_distance ) );

        ros::spinOnce();

    }

    void moveStop( void ) 
    {
        /* Set the velocity vector */
        geometry_msgs::Twist msg;

        msg.linear.x = 0.0;
        msg.linear.y = 0.0;
        msg.linear.z = 0.0;

        msg.angular.x = 0.0;
        msg.angular.y = 0.0;
        msg.angular.z = 0.0;

        robotMovement::turtle_pub.publish( msg );
        ros::spin();
    }

};

int main( int argc, char **argv ) {

    ros::init( argc, argv, "my_turtlebot" );

    robotMovement move( "cmd_vel", 0.8, 1.0, 10 );

    move.moveStraight( 1.0 );
    move.moveStop();

    return 0;
}

It compiles and works. BUT I realised soon that the robot is not moving 1 m (in RVIZ). So I run in 2 separate consoles the toll to debug tf and I get

• Console#1: running rosrun tf tf_echo /odom /base_footprint it displays a differnce between /odom and /base_footprint about 0.84 m

....................
At time 1407604810.474
Translation: [0.840, 0.000, 0.000]
Rotation: in ...