diff_drive_odom.cpp

// Very Simple Differential Drive Odometry. Ported from Python into C++
// Original Python: https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_python/src/ros_arduino_python/base_controller.py

double x_;
double y_;
double th_;
double dt; // time between last odometry calculation and this odometry calculation
long prev_left_enc_;
long prev_right_enc_;

// Calculate odometry
double dleft = (left_enc - prev_left_enc_) / ticks_per_meter;
double dright = (right_enc - prev_right_enc_) / ticks_per_meter;

prev_left_enc_ = left_enc;
prev_right_enc_ = right_enc;

double dxy_ave = (dright + dleft) / 2.0;
double dth = (dright - dleft) / wheel_track;
double vxy = dxy_ave / dt;
double vth = dth / dt;
  
if (dxy_ave != 0.0)
{
  dx = cos(dth) * dxy_ave;
  dy = -sin(dth) * dxy_ave;
  x_ += (cos(th_) * dx - sin(th_) * dy);
  self.y += (sin(th_) * dx + cos(th_) * dy);
}

if (dth != 0)
{
  th_ += dth;
}