Omni wheel drive odometry update

omni_wheel_drive_controller/CMakeLists.txt

@@ -85,6 +85,14 @@ if(BUILD_TESTING)
     controller_manager::controller_manager
     ros2_control_test_assets::ros2_control_test_assets
   )
+
+  ament_add_gmock(test_odometry
+    test/test_odometry.cpp
+    src/odometry.cpp
+  )
+  target_link_libraries(test_odometry
+    omni_wheel_drive_controller
+  )
 endif()
 
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)

omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp

@@ -27,12 +27,27 @@ class Odometry
 public:
   Odometry();
 
+  [[deprecated(
+    "Replaced by bool update_from_pos(const std::vector<double> & wheels_pos, double "
+    "dt).")]]
   bool updateFromPos(const std::vector<double> & wheels_pos, const rclcpp::Time & time);
+  [[deprecated(
+    "Replaced by bool update_from_vel(const std::vector<double> & wheels_vel, double "
+    "dt).")]]
   bool updateFromVel(const std::vector<double> & wheels_vel, const rclcpp::Time & time);
+  [[deprecated(
+    "Replaced by bool try_update_open_loop(const double & linear_x_vel, const double "
+    "& linear_y_vel, const double & angular_vel, double dt).")]]
   bool updateOpenLoop(
     const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
     const rclcpp::Time & time);
   [[deprecated("Use setOdometry(0.0, 0.0, 0.0) instead")]] void resetOdometry();
+
+  bool update_from_pos(const std::vector<double> & wheels_pos, double dt);
+  bool update_from_vel(const std::vector<double> & wheels_vel, double dt);
+  bool try_update_open_loop(
+    const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
+    double dt);
   void setOdometry(const double & x, const double & y, const double & heading);
 
   double getX() const { return x_; }
@@ -48,7 +63,9 @@ class Odometry
 
 private:
   Eigen::Vector3d compute_robot_velocity(const std::vector<double> & wheels_vel) const;
-  void integrate(const double & dx, const double & dy, const double & dheading);
+  void integrate(
+    const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
+    double dt);
 
   // Current timestamp:
   rclcpp::Time timestamp_;

omni_wheel_drive_controller/src/odometry.cpp

@@ -51,13 +51,36 @@ bool Odometry::updateFromPos(const std::vector<double> & wheels_pos, const rclcp
     wheels_old_pos_[i] = wheels_pos[i];
   }
 
+  // Disable deprecated warnings
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
   if (updateFromVel(wheels_vel, time))
+#pragma GCC diagnostic pop
   {
     return true;
   }
   return false;
 }
 
+bool Odometry::update_from_pos(const std::vector<double> & wheels_pos, double dt)
+{
+  // We cannot estimate angular velocity with very small time intervals
+  if (std::fabs(dt) < 1e-6)
+  {
+    return false;
+  }
+
+  // Estimate angular velocity of wheels using old and current position [rads/s]:
+  std::vector<double> wheels_vel(wheels_pos.size());
+  for (size_t i = 0; i < static_cast<size_t>(wheels_pos.size()); ++i)
+  {
+    wheels_vel[i] = (wheels_pos[i] - wheels_old_pos_[i]) / dt;
+    wheels_old_pos_[i] = wheels_pos[i];
+  }
+
+  return update_from_vel(wheels_vel, dt);
+}
+
 bool Odometry::updateFromVel(const std::vector<double> & wheels_vel, const rclcpp::Time & time)
 {
   const double dt = time.seconds() - timestamp_.seconds();
@@ -66,7 +89,7 @@ bool Odometry::updateFromVel(const std::vector<double> & wheels_vel, const rclcp
   const Eigen::Vector3d robot_velocity = compute_robot_velocity(wheels_vel);
 
   // Integrate odometry:
-  integrate(robot_velocity(0) * dt, robot_velocity(1) * dt, robot_velocity(2) * dt);
+  integrate(robot_velocity(0), robot_velocity(1), robot_velocity(2), dt);
 
   timestamp_ = time;
 
@@ -77,6 +100,26 @@ bool Odometry::updateFromVel(const std::vector<double> & wheels_vel, const rclcp
   return true;
 }
 
+bool Odometry::update_from_vel(const std::vector<double> & wheels_vel, double dt)
+{
+  if (std::fabs(dt) < 1e-6)
+  {
+    return false;
+  }
+
+  // Compute linear and angular velocities of the robot:
+  const Eigen::Vector3d robot_velocity = compute_robot_velocity(wheels_vel);
+
+  // Integrate odometry:
+  integrate(robot_velocity(0), robot_velocity(1), robot_velocity(2), dt);
+
+  linear_x_vel_ = robot_velocity(0);
+  linear_y_vel_ = robot_velocity(1);
+  angular_vel_ = robot_velocity(2);
+
+  return true;
+}
+
 Eigen::Vector3d Odometry::compute_robot_velocity(const std::vector<double> & wheels_vel) const
 {
   Eigen::MatrixXd A(wheels_vel.size(), 3);   // Transformation Matrix
@@ -110,7 +153,7 @@ bool Odometry::updateOpenLoop(
   const double dt = time.seconds() - timestamp_.seconds();
 
   // Integrate odometry:
-  integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt);
+  integrate(linear_x_vel, linear_y_vel, angular_vel, dt);
 
   timestamp_ = time;
 
@@ -122,6 +165,20 @@ bool Odometry::updateOpenLoop(
   return true;
 }
 
+bool Odometry::try_update_open_loop(
+  const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, double dt)
+{
+  // Integrate odometry:
+  integrate(linear_x_vel, linear_y_vel, angular_vel, dt);
+
+  // Save last linear and angular velocity:
+  linear_x_vel_ = linear_x_vel;
+  linear_y_vel_ = linear_y_vel;
+  angular_vel_ = angular_vel;
+
+  return true;
+}
+
 void Odometry::resetOdometry()
 {
   x_ = 0.0;
@@ -145,8 +202,18 @@ void Odometry::setParams(
   wheels_old_pos_.resize(wheel_count, 0.0);
 }
 
-void Odometry::integrate(const double & dx, const double & dy, const double & dheading)
+void Odometry::integrate(
+  const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, double dt)
 {
+  if (std::fabs(dt) < 1e-6)
+  {
+    return;
+  }
+
+  const double dx = linear_x_vel * dt;
+  const double dy = linear_y_vel * dt;
+  const double dheading = angular_vel * dt;
+
   if (std::fabs(dheading) < 1e-6)
   {
     // For very small dheading, approximate to linear motion

omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp

@@ -312,7 +312,7 @@ controller_interface::return_type OmniWheelDriveController::update_reference_fro
 }
 
 controller_interface::return_type OmniWheelDriveController::update_and_write_commands(
-  const rclcpp::Time & time, const rclcpp::Duration &)
+  const rclcpp::Time &, const rclcpp::Duration & period)
 {
   rclcpp::Logger logger = get_node()->get_logger();
 
@@ -343,8 +343,9 @@ controller_interface::return_type OmniWheelDriveController::update_and_write_com
     // Update odometry
     if (params_.open_loop)
     {
-      odometry_updated = odometry_.updateOpenLoop(
-        reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time);
+      odometry_updated = odometry_.try_update_open_loop(
+        reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2],
+        period.seconds());
     }
     else
     {
@@ -371,11 +372,11 @@ controller_interface::return_type OmniWheelDriveController::update_and_write_com
       }
       if (params_.position_feedback)
       {
-        odometry_updated = odometry_.updateFromPos(wheels_feedback, time);
+        odometry_updated = odometry_.update_from_pos(wheels_feedback, period.seconds());
       }
       else
       {
-        odometry_updated = odometry_.updateFromVel(wheels_feedback, time);
+        odometry_updated = odometry_.update_from_vel(wheels_feedback, period.seconds());
       }
     }
   }

omni_wheel_drive_controller/test/test_odometry.cpp

@@ -0,0 +1,159 @@
+// Copyright 2026 Devdoot Chatterjee
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gmock/gmock.h"
+#include "omni_wheel_drive_controller/odometry.hpp"
+
+class OmniOdometryTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    // Standard 4-wheel omni robot
+    // Robot radius = 1.0m, Wheel radius = 0.1m, Wheel offset = 0.0 rads, 4 wheels
+    odometry_.setParams(1.0, 0.1, 0.0, 4);
+  }
+
+  omni_wheel_drive_controller::Odometry odometry_;
+};
+
+TEST_F(OmniOdometryTest, TestInitialState)
+{
+  EXPECT_DOUBLE_EQ(odometry_.getX(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getY(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getHeading(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getLinearXVel(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getLinearYVel(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getAngularVel(), 0.0);
+}
+
+TEST_F(OmniOdometryTest, TestLinearMotionX)
+{
+  // Move purely forward in X: Vx = 1.0, Vy = 0.0, W = 0.0, dt = 1.0
+  // W0 = 0, W1 = 10, W2 = 0, W3 = -10
+  std::vector<double> wheel_vels = {0.0, 10.0, 0.0, -10.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1.0);
+
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(odometry_.getX(), 1.0, 1e-5);
+  EXPECT_NEAR(odometry_.getY(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getHeading(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getLinearXVel(), 1.0, 1e-5);
+  EXPECT_NEAR(odometry_.getLinearYVel(), 0.0, 1e-5);
+}
+
+TEST_F(OmniOdometryTest, TestLinearMotionY)
+{
+  // Strafing purely sideways in Y: Vx = 0.0, Vy = 1.0, W = 0.0, dt = 1.0
+  // W0 = -10, W1 = 0, W2 = 10, W3 = 0
+  std::vector<double> wheel_vels = {-10.0, 0.0, 10.0, 0.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1.0);
+
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(odometry_.getX(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getY(), 1.0, 1e-5);
+  EXPECT_NEAR(odometry_.getHeading(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getLinearYVel(), 1.0, 1e-5);
+}
+
+TEST_F(OmniOdometryTest, TestPureRotation)
+{
+  // Rotate in place: Vx = 0.0, Vy = 0.0, W = 2.0, dt = 1.0
+  // W0 = -20, W1 = -20, W2 = -20, W3 = -20
+  std::vector<double> wheel_vels = {-20.0, -20.0, -20.0, -20.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1.0);
+
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(odometry_.getX(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getY(), 0.0, 1e-5);
+  EXPECT_NEAR(odometry_.getHeading(), 2.0, 1e-5);
+  EXPECT_NEAR(odometry_.getAngularVel(), 2.0, 1e-5);
+}
+
+TEST_F(OmniOdometryTest, TestCurvedMotion_ExactArc)
+{
+  // Curve: Moving in X, Y, and Rotating simultaneously
+  // Vx = 1.5, Vy = 0.5, W = 1.0, dt = 1.0
+  // W0 = (-0.5 - 1.0)*10 = -15
+  // W1 = (1.5 - 1.0)*10 = 5
+  // W2 = (0.5 - 1.0)*10 = -5
+  // W3 = (-1.5 - 1.0)*10 = -25
+  std::vector<double> wheel_vels = {-15.0, 5.0, -5.0, -25.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1.0);
+  EXPECT_TRUE(result);
+
+  const double expected_x = (1.5 / 1.0) * std::sin(1.0) + (0.5 / 1.0) * (std::cos(1.0) - 1.0);
+  const double expected_y = -(1.5 / 1.0) * (std::cos(1.0) - 1.0) + (0.5 / 1.0) * std::sin(1.0);
+
+  EXPECT_NEAR(odometry_.getX(), expected_x, 1e-5);
+  EXPECT_NEAR(odometry_.getY(), expected_y, 1e-5);
+  EXPECT_NEAR(odometry_.getHeading(), 1.0, 1e-5);
+}
+
+TEST_F(OmniOdometryTest, TestSmallDtRejection)
+{
+  std::vector<double> wheel_vels = {1.0, 1.0, 1.0, 1.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1e-7);
+
+  EXPECT_FALSE(result);
+  EXPECT_DOUBLE_EQ(odometry_.getX(), 0.0);
+}
+
+TEST_F(OmniOdometryTest, TestOpenLoopUpdate)
+{
+  // Directly feed vx=2.0, vy=0.5, w=1.0, dt=1.0 to bypass SVD math
+  bool result = odometry_.try_update_open_loop(2.0, 0.5, 1.0, 1.0);
+
+  EXPECT_TRUE(result);
+
+  const double expected_x = (2.0 / 1.0) * std::sin(1.0) + (0.5 / 1.0) * (std::cos(1.0) - 1.0);
+  const double expected_y = -(2.0 / 1.0) * (std::cos(1.0) - 1.0) + (0.5 / 1.0) * std::sin(1.0);
+
+  EXPECT_NEAR(odometry_.getX(), expected_x, 1e-5);
+  EXPECT_NEAR(odometry_.getY(), expected_y, 1e-5);
+  EXPECT_DOUBLE_EQ(odometry_.getHeading(), 1.0);
+
+  EXPECT_DOUBLE_EQ(odometry_.getLinearXVel(), 2.0);
+  EXPECT_DOUBLE_EQ(odometry_.getLinearYVel(), 0.5);
+  EXPECT_DOUBLE_EQ(odometry_.getAngularVel(), 1.0);
+}
+
+TEST_F(OmniOdometryTest, TestUpdateFromPosition)
+{
+  // Feed position increments that equate to Vx=1.0 over dt=1.0
+  std::vector<double> wheel_pos = {0.0, 10.0, 0.0, -10.0};
+
+  bool result = odometry_.update_from_pos(wheel_pos, 1.0);
+
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(odometry_.getX(), 1.0, 1e-5);
+  EXPECT_NEAR(odometry_.getLinearXVel(), 1.0, 1e-5);
+}
+
+TEST_F(OmniOdometryTest, TestReset)
+{
+  // 1. Move the robot
+  std::vector<double> wheel_vels = {0.0, 10.0, 0.0, -10.0};
+  bool result = odometry_.update_from_vel(wheel_vels, 1.0);
+  EXPECT_TRUE(result);
+  EXPECT_NE(odometry_.getX(), 0.0);
+
+  // 2. Reset
+  odometry_.setOdometry(0.0, 0.0, 0.0);
+
+  // 3. Verify position is cleared
+  EXPECT_DOUBLE_EQ(odometry_.getX(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getY(), 0.0);
+  EXPECT_DOUBLE_EQ(odometry_.getHeading(), 0.0);
+}