Fix computation: ros-controls/ros2_controllers#1481

Author: rafal-gorecki

mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp

@@ -27,8 +27,6 @@
 #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_
 #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_
 
-#include <cmath>
-
 #include "rclcpp/time.hpp"
 #include "rcpputils/rolling_mean_accumulator.hpp"
 

mecanum_drive_controller/src/odometry.cpp

@@ -21,6 +21,8 @@
 // Based on: https://ecam-eurobot.github.io/Tutorials/mechanical/mecanum.html
 // Author: Maciej Stępień
 
+#include <cmath>
+
 #include "mecanum_drive_controller/odometry.hpp"
 
 namespace mecanum_drive_controller
@@ -94,7 +96,11 @@ bool Odometry::updateFromVelocity(
   const rclcpp::Time & time)
 {
   const double dt = time.seconds() - timestamp_.seconds();
-
+  if (dt < 0.0001)
+  {
+    return false;  // Interval too small to integrate with
+  }
+  // Compute linear and angular diff:
   const double linear_x = (front_left_vel + front_right_vel + rear_left_vel + rear_right_vel) / 4.;
   const double linear_y = (-front_left_vel + front_right_vel + rear_left_vel - rear_right_vel) / 4.;
   const double angular = (-front_left_vel + front_right_vel - rear_left_vel + rear_right_vel) /