Revision history [back]

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.