# How to calculate odometry using encoders?

I have tried all the solutions but can't seem to make it work, I can see the odometry in rviz with respect to the /odom frame, and see the tf go around in circles but it doesn't move with respect to the base_link frame. When I try to plot it in MATLAB, I get this plot, this is the robot going straight.

Complete code:Odometry.cpp

    void update_velocities(
double dt)
if (dt > 0) {
int curr_encoder_count_left =
current_encoder_count_left;
int curr_encoder_count_right =
current_encoder_count_right;
{
double cos_current = cos(theta);
double sin_current = sin(theta);

// relying on the compiler for wraparound here
int dist_left_counts =
curr_encoder_count_left -
previous_encoder_count_left;
int dist_right_counts =
curr_encoder_count_right -
previous_encoder_count_right;

if (dist_left_counts != dist_right_counts) {
// convert from counts to standard units
double dist_left_mm =
(double)dist_left_counts /
left_encoder_counts_per_mm;
double dist_right_mm =
(double)dist_right_counts /
right_encoder_counts_per_mm;

double right_left = dist_right_mm - dist_left_mm;

double a =
wheelbase_mm * (dist_right_mm + dist_left_mm) *
0.5 / right_left;

double fraction = right_left / wheelbase_mm;
const double mm_to_m = 1.0 / 1000;
vx = a * (sin(fraction + theta) - sin_current) *
mm_to_m;
vy = -a * (cos(fraction + theta) - cos_current) *
mm_to_m;
vtheta = fraction;

// ROS_INFO("pose %.3f %.3f orientation %.3f",
//  (float)x, (float)y, (float)theta);
ROS_DEBUG("Encoders %.3f %.3f",
dist_left_mm, dist_right_mm);
}
else {
vx=vy=vtheta=0;
}

// store previous values
previous_encoder_count_left = curr_encoder_count_left;
previous_encoder_count_right = curr_encoder_count_right;
}
}

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

ros::NodeHandle n;
ros::NodeHandle nh("~");

std::string name = "odom";
nh.getParam("name", name);

std::string reset_topic = "odometry/reset";
nh.getParam("reset_topic", reset_topic);

nh.getParam("rotation", rotation_offset);

n.setParam(reset_topic, false);

ros::Publisher odom_pub =

// get encoder indexes
encoder_index_left = -1;
nh.getParam("encoderindexleft", encoder_index_left);
encoder_index_right = -1;
nh.getParam("encoderindexright", encoder_index_right);

int direction = -1;
nh.getParam("encoderdirectionleft", direction);
if (direction!=-1) encoder_direction_left = 1;
direction=1;
nh.getParam("encoderdirectionright",
encoder_direction_right);
if (direction!=1) encoder_direction_right = -1;

// connect to the encoders
bool subscribed_to_encoders = false;
if ((encoder_index_left > 0) &&
(encoder_index_right > 0) &&
(encoder_index_left!=encoder_index_right)) {
subscribed_to_encoders =
subscribe_to_encoders_by_index();
}
else {
subscribed_to_encoders = subscribe_to_encoders();
}

if (subscribed_to_encoders) {

std::string frame_id = "odom";
nh.getParam("frame_id", frame_id);
left_encoder_counts_per_mm = 0;
right_encoder_counts_per_mm = 0;
nh.getParam("countspermmleft",
left_encoder_counts_per_mm);
if (left_encoder_counts_per_mm < 1) {
left_encoder_counts_per_mm = 1000;
}
nh.getParam("countspermmright",
right_encoder_counts_per_mm);
if (right_encoder_counts_per_mm < 1) {
right_encoder_counts_per_mm = 1000;
}
wheelbase_mm = 0;
nh.getParam("wheelbase", wheelbase_mm);
if (wheelbase_mm < 1) wheelbase_mm = 400;

nh.getParam("verbose", verbose);
nh.setParam("verbose", false);

int frequency = 25;
nh.getParam("frequency", frequency);

ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();

initialised = true;

ros::Rate update_rate(frequency);
while(ros::ok()){
// reset the pose
bool reset = false;
n.getParam(reset_topic, reset);
if (reset) {
x = 0;
y = 0;
theta = 0;
vx = 0;
vy = 0;
vtheta = 0;
n.setParam(reset_topic, false);
start_encoder_count_left = 0;
start_encoder_count_right = 0;
}

current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();

// update the velocity estimate based upon
// encoder values
update_velocities(dt);

// compute odometry in a typical way given
// the velocities of the robot
x += vx;
y += vy;
theta += vtheta;

// since all odometry is 6DOF we'll need a
// quaternion created from yaw
geometry_msgs::Quaternion odom_quat =
tf::createQuaternionMsgFromYaw(theta);

// first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;

odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

// send the transform

// next, we'll publish the odometry
//  message over ROS
nav_msgs::Odometry odom;

// set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

// set the velocity
odom.twist.twist.linear.x = vx/dt;
odom.twist.twist.linear.y = vy/dt;
odom.twist.twist.angular.z = vtheta/dt;

// publish the message
odom_pub.publish(odom);

last_time = current_time;
ros::spinOnce();
update_rate.sleep();
}
}
}
}


Originally posted by fiorano10 on ROS Answers with karma: 45 on 2017-09-20

Post score: 0

I'm not sure if this is your problem but why do you only compute the odometry in update_velocities() when your encoder counts for each wheel are different? ie: if (dist_left_counts != dist_right_counts) { ... }

If you are driving in a straight line isn't it possible that your left and right wheels would have the same number of encoder counts?

   const double mm_to_m = 1.0 / 1000.0;
double wheelbase_m = wheelbase_mm * mm_to_m; //to avoid issues
double right_left = (dist_right_mm - dist_left_mm) * mm_to_m; //get this is meters

double a =  (dist_right_mm + dist_left_mm) * 0.5 * mm_to_m ; //get this is meters

double fraction = right_left / wheelbase_m;

//assuming theta is the previous yaw value
vx = a * cos(theta + (fraction/2.0));
vy = a * sin(theta + (fraction/2.0));
vtheta = fraction;


This is the standard diff drive odom model. I believe you may have had issues from units and your cos and sin were backwards.

Originally posted by biglotusturtle with karma: 165 on 2017-09-21

This answer was ACCEPTED on the original site

Post score: 0

Comment by fiorano10 on 2017-09-21:
It is possible so I tried running it without that condition and I'm still getting the same output, I can see the tf and odometry revolve in circles but they don't move in with respect to base link, could it be a transform issue ? I've added the tf tree

Comment by biglotusturtle on 2017-09-21:
May be a tf issue. Print your x,y, and theta values out to the command window and check that the values look reasonable. Also try printing out your wheel velocity values and do a sanity check on those. Maybe one of your encoders is bad?

Comment by fiorano10 on 2017-09-21:
Can you take a look at this Odometry_data

Comment by biglotusturtle on 2017-09-21:
Looks like your velocities are almost always 0.0. Try getting rid of that reset condition/param in your while loop? Also what is the output of your ROS_DEBUG("Encoders %.3f %.3f",dist_left_mm, dist_right_mm); ? Do your wheel distances seem reasonable?

Comment by fiorano10 on 2017-09-25:
I got rid of the reset condition, made the changes you suggested in EDIT 1, I'm getting this https://youtu.be/oeEnLN81uJU. Also, when I display messages in rqt_console, I can't see Debug messages ? How do I display the distance ?

Comment by biglotusturtle on 2017-09-26:
Change your print statement to ROS_INFO("Encoders %.3f %.3f", dist_left_mm, dist_right_mm);

And make sure output="screen" in your launch file

Based on the video it is looking like you encoder are returning bad data.

Comment by fiorano10 on 2017-09-26: