Revision history [back]

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