ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# 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 ...
edit retag close merge delete

Sort by ยป oldest newest most voted

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?

EDIT 1: Upon looking at your code I believe your math might be wrong in your wheel_velocities(). Try this instead

   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.

more

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

( 2017-09-21 12:09:50 -0500 )edit

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?

( 2017-09-21 12:17:14 -0500 )edit

Can you take a look at this Odometry_data

( 2017-09-21 14:25:16 -0500 )edit

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?

( 2017-09-21 14:37:12 -0500 )edit

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 ?

( 2017-09-25 15:28:56 -0500 )edit

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.

( 2017-09-26 12:16:31 -0500 )edit
( 2017-09-26 14:01:33 -0500 )edit

How often is the dist_left_mm and dist_right_mm being computed? I am seeing values of 10 mm? Which would be the same as 0.3 m/s at 30 hz? Does that seem reasonable for your wheel sizes? Also you wont necessarily see data in rqt-reconfigure. Ros_info should show up in your command window.

( 2017-09-26 20:34:59 -0500 )edit

## Stats

Asked: 2017-09-20 18:11:03 -0500

Seen: 5,811 times

Last updated: Nov 15 '17