Ask Your Question

Does an finished odometry node exists?

asked 2018-06-13 04:29:51 -0500

-LD- gravatar image

Hello everybody.

I'm new to ROS. I finished some of the beginner tutorials and than started to work on my robot platform from the university. Its a pretty easy setup with two powered wheels and one passive wheel for stabilization.

I was surprised that some of the nodes existed already. Like the joy_node for publishing all the data from the wireless joystick i have. And also the SICK-Node to read out the data from the laser scanner.

After writing my own node for the elmo controllers i thought about starting with the odometry. But well, to be honest, each time i look at the code of someone knowing what he/she is doing, my own code looks like building something with Lego bricks instead of using the 3D printer. That's also the problem with the code spinets i found so far. I always couldn't understand parts of them.

So, the question is, is there a finished node for this pretty standard kinematic? Just like the node for the joystick, which publishes the data on a topic. Here it would be publishing the estimated position just by feeding in the geometric dimensions (wheel diameter, wheel distance, counts per turn) and the encoder values. I'm pretty sure i'm not the first one needing something like this. But so far i couldn't find something finished as a standalone node.

Thanks for the time of everybody reading till here :D

Best regards, Lukas

Using: Linux 64, ROS Indigo

edit retag flag offensive close merge delete


You might want to take a look at diff_drive_controller or the differential_drive package

Humpelstilzchen gravatar imageHumpelstilzchen ( 2018-06-15 01:34:59 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2018-06-14 20:57:18 -0500

chrisalbertson gravatar image

There are many. But you want one that works and is simple. Just last night I was browsing code in the ROS Serial package. There is example code there and one is an odometer node. This example generates its own fake X, Y and Theta values but you can get your own from wheel encoders.

The trick: Given the rotation done in the last time period (Call it delta-Theta) the rest is easy. But how to calc. rotation? The key is that rotation is the difference in wheel speed. Subtract min(left, right) from both left and right and one speed will be zero. I assume you can calc rotation in the special case where one wheel is locked. So convert all cases to the spacial case you can solve by assuming one wheel is moving at the difference in speed and the other is locked. then assume the center of your robot tracks the average wheel speed in X and Y using the average heading over the time period.

Every Delta-T you find deltaTheta, dX and dY and add those to T,X, Y and report those in the message.

I think the trick really is to talk the entire process out loud BEFORE you write even a line of code

edit flag offensive delete link more


@chrisalbertson Are you referring to this example?

jayess gravatar imagejayess ( 2018-06-16 17:45:43 -0500 )edit

answered 2018-06-15 09:50:52 -0500

-LD- gravatar image


i ended up doing it by myself. And since maybe someone is interested in (or what to make suggestions). Anything i could/should change ?

#include "ros/ros.h"                                    
#include "std_msgs/Int64MultiArray.h"   
#include "geometry_msgs/Vector3.h"      
#include "math.h"               

// -------------------------------------------------------------

// prototype + global Var
void encoderCallback(std_msgs::Int64MultiArray array);
#define TIME        0
#define E_L         1
#define E_R         2

bool new_data;

geometry_msgs::Vector3 encoder;     // time, left, right
#define ENC_T               encoder.x
#define ENC_L               encoder.y
#define ENC_R               encoder.z

geometry_msgs::Vector3 encoder_new;// time, left, right
#define ENC_T_NEW       encoder_new.x
#define ENC_L_NEW       encoder_new.y
#define ENC_R_NEW       encoder_new.z

geometry_msgs::Vector3 position;        // x, y, theta
#define POS_X               position.x
#define POS_Y               position.y
#define POS_THETA           position.z

#define R   75      // [mm] wheel radius
#define D   530     // [mm] distance between wheels
#define C   148000  // [] counts per turn, ecperimental value 

// -------------------------------------------------------------

int main(int argc, char **argv)

    // init node
    ros::init(argc, argv, "OdometryNode");      
    ros::NodeHandle n;                  
    ros::Rate loop_rate(10);    

    // init topics
    ros::Publisher  Position  = n.advertise<geometry_msgs::Vector3>("Odometry_Pos", 1);
    ros::Subscriber Encoders  = n.subscribe<std_msgs::Int64MultiArray>("MotorDrive_encoder", 1, encoderCallback);

    // init global vars
    POS_X       = 0;
    POS_Y       = 0;
    POS_THETA   = 0;
    new_data    = false;

    // wait for first encoder values arrive
    while (new_data == false && ros::ok()){
    ENC_T = ENC_T_NEW;
    ENC_L = ENC_L_NEW;
    ENC_R = ENC_R_NEW;
    new_data = false;

    // values independent from encoders
    float M_PI_2 = 2.0*M_PI;
    float d_2 = D/2;
    float r_PI_c = R*2*M_PI/C;          // orientation in: rad (=2*PI) or deg (=360)
    float r_PI_d_c = R*2*M_PI/(D*C);    // constant for the angle

    while (ros::ok())
        // just when needed
        if(new_data == true)
            // get driven distance in counts
            long e_l = ENC_L_NEW - ENC_L;
            long e_r = ENC_R_NEW - ENC_R;
            ROS_INFO("e_l %li, e_r %li", e_l, e_r);

            // save new data as current ones
            ENC_T = ENC_T_NEW;
            ENC_L = ENC_L_NEW;
            ENC_R = ENC_R_NEW;

            // rotation + maybe transation
            float dx, dy, alpha;
            if((e_l - e_r) > 10 || (e_l - e_r) < -10)
                // radius + angle of the movement   
                float e_temp = (e_l + e_r) / (e_l - e_r);
                float r = d_2 * e_temp;
                alpha = (e_l - e_r) * r_PI_d_c;

                // move in robot frame
                dx = r * sin(alpha);
                dy = r * (1-cos(alpha));

            else    // just translation
                dx = e_l * r_PI_c;
                dy = 0.00;
                alpha = 0.00;

            // prepare transformation
            float temp_cos = cos(POS_THETA);
            float temp_sin = sin(POS_THETA);

            // transform to global frame (rotation matrix)
            POS_X       += dx*temp_cos - dy*temp_sin;
            POS_Y       += dx*temp_sin + dy*temp_cos;
            POS_THETA    += alpha;
            POS_THETA   %= M_PI_2;

            // publish new estimate of position
            new_data = false;

        // tell ROS to repeat

    return 0;

// -------------------------------------------------------------

void encoderCallback(std_msgs::Int64MultiArray data_new)
    // just save the data, rest is in main loop
    ENC_T_NEW =[TIME];
    ENC_L_NEW =[E_L];
    ENC_R_NEW =[E_R];

    // flag to process new data
    new_data = true;
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2018-06-13 04:29:51 -0500

Seen: 52 times

Last updated: Jun 14 '18