Skip to content

Instantly share code, notes, and snippets.

@clungzta

clungzta/diff_drive_odom.cpp

Created July 16, 2021 05:44
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save clungzta/d0c3bfe5541b462063d2f0d6fec76dcb to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save clungzta/d0c3bfe5541b462063d2f0d6fec76dcb to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. clungzta created this gist Jul 16, 2021.
    34 changes: 34 additions & 0 deletions diff_drive_odom.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,34 @@
    // 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;
    }