Updated diff_drive odometry class to consistantly

use velocity where it claims to, and pass timestep
in to every method that makes use of it.

This was requested in ros-controls#271.

- The result is that the class is very slightly less efficient (but
still quite trivial) due to passing around a extra double.

- The other result is that it is no longer possible for velocity based
odometry updates calculated incorrectly when the timestep fails to keep up.

To prevent position based updates from now suffering the inverse of
the velocity timestep/dt mismatch, the measured delta is used exclusively
for position based updates (retaining the existing behavior), while the
configured timestep is used for open and closed loop velocity updates.

I also added a 1s throttled warning if there is a major mismatch between
measured and configured timestep (+/-20%) since this would cause a slight
accumulation of integration errors on curved paths (I believe), depending
on velocity, curvature and mismatch size.

Similar requested period/measured period mismatches may be a nuisance for
open loop (which is velocity based) and closed loop velocity based updates,
but the impact is likely negligible (in this class).

Author: josephduchesne

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -40,9 +40,9 @@ class Odometry
   explicit Odometry(size_t velocity_rolling_window_size = 10);
 
   void init(const rclcpp::Time & time);
-  bool update(double left_pos, double right_pos, const rclcpp::Time & time);
-  bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time);
-  void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);
+  bool updateFromPosition(double left_pos, double right_pos, const rclcpp::Time & time, const double dt);
+  bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time, const double dt);
+  void updateFromVelocityOpenLoop(double linear, double angular, const rclcpp::Time & time, const double dt);
   void resetOdometry();
 
   double getX() const { return x_; }
@@ -62,8 +62,8 @@ class Odometry
   using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
 #endif
 
-  void integrateRungeKutta2(double linear, double angular);
-  void integrateExact(double linear, double angular);
+  void integrateRungeKutta2(double linear, double angular, const double dt);
+  void integrateExact(double linear, double angular, const double dt);
   void resetAccumulators();
 
   // Current timestamp:

diff_drive_controller/src/diff_drive_controller.cpp

@@ -141,9 +141,10 @@ controller_interface::return_type DiffDriveController::update(
   const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
   const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;
 
+
   if (params_.open_loop)
   {
-    odometry_.updateOpenLoop(linear_command, angular_command, time);
+    odometry_.updateFromVelocityOpenLoop(linear_command, angular_command, time, period.seconds());
   }
   else
   {
@@ -171,13 +172,28 @@ controller_interface::return_type DiffDriveController::update(
 
     if (params_.position_feedback)
     {
-      odometry_.update(left_feedback_mean, right_feedback_mean, time);
+      // Warn if actual timestep doesn't match period, 
+      // since 
+      const double measured_period = (
+        get_node()->get_clock()->now() - previous_update_timestamp_).seconds();
+      
+      // +/- 20% of nominal loop time as a starting point to warn wheel encoder
+      // users that the real update rate doesn't match the requested update period
+      if ( std::fabs(period.seconds()-measured_period) > period.seconds()/5.0 ) {
+        RCLCPP_WARN_THROTTLE(
+          get_node()->get_logger(), *(get_node()->get_clock()), 1000,
+          "Integrating odometry at measured delta of %.3fs, but desired update period is %.3fs",
+          measured_period, period.seconds());
+      }
+
+      // update odometry 
+      odometry_.updateFromPosition(left_feedback_mean, right_feedback_mean, time, measured_period);
     }
     else
     {
       odometry_.updateFromVelocity(
-        left_feedback_mean * left_wheel_radius * period.seconds(),
-        right_feedback_mean * right_wheel_radius * period.seconds(), time);
+        left_feedback_mean * left_wheel_radius,
+        right_feedback_mean * right_wheel_radius, time, period.seconds());
     }
   }
 

diff_drive_controller/src/odometry.cpp

@@ -45,10 +45,9 @@ void Odometry::init(const rclcpp::Time & time)
   timestamp_ = time;
 }
 
-bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & time)
+bool Odometry::updateFromPosition(double left_pos, double right_pos, const rclcpp::Time & time, const double dt)
 {
   // We cannot estimate the speed with very small time intervals:
-  const double dt = time.seconds() - timestamp_.seconds();
   if (dt < 0.0001)
   {
     return false;  // Interval too small to integrate with
@@ -59,52 +58,49 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
   const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
 
   // Estimate velocity of wheels using old and current position:
-  const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
-  const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
+  const double left_wheel_est_vel = (left_wheel_cur_pos - left_wheel_old_pos_)/dt;
+  const double right_wheel_est_vel = (right_wheel_cur_pos - right_wheel_old_pos_)/dt;
 
   // Update old position with current:
   left_wheel_old_pos_ = left_wheel_cur_pos;
   right_wheel_old_pos_ = right_wheel_cur_pos;
 
-  updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time);
+  updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time, dt);
 
   return true;
 }
 
-bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
+bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time, const double dt)
 {
-  const double dt = time.seconds() - timestamp_.seconds();
-
   // Compute linear and angular diff:
   const double linear = (left_vel + right_vel) * 0.5;
   // Now there is a bug about scout angular velocity
   const double angular = (right_vel - left_vel) / wheel_separation_;
 
   // Integrate odometry:
-  integrateExact(linear, angular);
+  integrateExact(linear, angular, dt);
 
   timestamp_ = time;
 
   // Estimate speeds using a rolling mean to filter them out:
-  linear_accumulator_.accumulate(linear / dt);
-  angular_accumulator_.accumulate(angular / dt);
+  linear_accumulator_.accumulate(linear);
+  angular_accumulator_.accumulate(angular);
 
   linear_ = linear_accumulator_.getRollingMean();
   angular_ = angular_accumulator_.getRollingMean();
 
   return true;
 }
 
-void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time)
+void Odometry::updateFromVelocityOpenLoop(double linear, double angular, const rclcpp::Time & time, const double dt)
 {
   /// Save last linear and angular velocity:
   linear_ = linear;
   angular_ = angular;
 
   /// Integrate odometry:
-  const double dt = time.seconds() - timestamp_.seconds();
   timestamp_ = time;
-  integrateExact(linear * dt, angular * dt);
+  integrateExact(linear, angular, dt);
 }
 
 void Odometry::resetOdometry()
@@ -129,28 +125,28 @@ void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
   resetAccumulators();
 }
 
-void Odometry::integrateRungeKutta2(double linear, double angular)
+void Odometry::integrateRungeKutta2(double linear, double angular, const double dt)
 {
-  const double direction = heading_ + angular * 0.5;
+  const double direction = heading_ + angular * 0.5 * dt;
 
   /// Runge-Kutta 2nd order integration:
-  x_ += linear * cos(direction);
-  y_ += linear * sin(direction);
-  heading_ += angular;
+  x_ += linear * cos(direction) * dt;
+  y_ += linear * sin(direction) * dt;
+  heading_ += angular * dt;
 }
 
-void Odometry::integrateExact(double linear, double angular)
+void Odometry::integrateExact(double linear, double angular, const double dt)
 {
-  if (fabs(angular) < 1e-6)
+  if (fabs(angular*dt) < 1e-6)
   {
-    integrateRungeKutta2(linear, angular);
+    integrateRungeKutta2(linear, angular, dt);
   }
   else
   {
     /// Exact integration (should solve problems when angular is zero):
     const double heading_old = heading_;
-    const double r = linear / angular;
-    heading_ += angular;
+    const double r = linear / angular * dt;
+    heading_ += angular * dt;
     x_ += r * (sin(heading_) - sin(heading_old));
     y_ += -r * (cos(heading_) - cos(heading_old));
   }