# Does an finished odometry node exists?

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 close merge delete

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

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

Sort by » oldest newest most voted

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

more

@chrisalbertson Are you referring to this example?

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

Hello,

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()){
ros::spinOnce();
loop_rate.sleep();}
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
Position.publish(position);
new_data = false;
}

// tell ROS to repeat
ros::spinOnce();
loop_rate.sleep();
}

return 0;
}

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

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

// flag to process new data
new_data = true;
return;
}

more