Update odometry implementation in diff_drive

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -37,10 +37,25 @@ class Odometry
 public:
   explicit Odometry(size_t velocity_rolling_window_size = 10);
 
+  [[deprecated]]
   void init(const rclcpp::Time & time);
+  [[deprecated(
+    "Replaced by bool update_from_pos(double left_pos, double right_pos, const "
+    "rclcpp::Time & time).")]]
   bool update(double left_pos, double right_pos, const rclcpp::Time & time);
+  [[deprecated(
+    "Replaced by bool update_from_vel(double left_vel, double right_vel, const "
+    "rclcpp::Time & time).")]]
   bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time);
+  [[deprecated(
+    "Replaced by bool try_update_open_loop(double linear_vel, double angular_vel, const "
+    "rclcpp::Time "
+    "& time).")]]
   void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);
+
+  bool update_from_pos(double left_pos, double right_pos, double dt);
+  bool update_from_vel(double left_vel, double right_vel, double dt);
+  bool try_update_open_loop(double linear_vel, double angular_vel, double dt);
   void resetOdometry();
 
   double getX() const { return x_; }
@@ -60,8 +75,12 @@ class Odometry
   using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
 #endif
 
+  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
   void integrateRungeKutta2(double linear, double angular);
+  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
   void integrateExact(double linear, double angular);
+
+  void integrate(double linear_vel, double angular_vel, double dt);
   void resetAccumulators();
 
   // Current timestamp:

diff_drive_controller/src/diff_drive_controller.cpp

@@ -160,9 +160,12 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   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;
 
+  // Update odometry
+  bool odometry_updated = false;
   if (params_.open_loop)
   {
-    odometry_.updateOpenLoop(linear_command, angular_command, time);
+    odometry_updated =
+      odometry_.try_update_open_loop(linear_command, angular_command, period.seconds());
   }
   else
   {
@@ -200,62 +203,65 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
 
     if (params_.position_feedback)
     {
-      odometry_.update(left_feedback_mean, right_feedback_mean, time);
+      odometry_updated =
+        odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds());
     }
     else
     {
-      odometry_.updateFromVelocity(
-        left_feedback_mean * left_wheel_radius * period.seconds(),
-        right_feedback_mean * right_wheel_radius * period.seconds(), time);
+      odometry_updated =
+        odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds());
     }
   }
 
-  tf2::Quaternion orientation;
-  orientation.setRPY(0.0, 0.0, odometry_.getHeading());
-
-  bool should_publish = false;
-  try
+  if (odometry_updated)
   {
-    if (previous_publish_timestamp_ + publish_period_ < time)
+    tf2::Quaternion orientation;
+    orientation.setRPY(0.0, 0.0, odometry_.getHeading());
+
+    bool should_publish = false;
+    try
     {
-      previous_publish_timestamp_ += publish_period_;
-      should_publish = true;
+      if (previous_publish_timestamp_ + publish_period_ < time)
+      {
+        previous_publish_timestamp_ += publish_period_;
+        should_publish = true;
+      }
     }
-  }
-  catch (const std::runtime_error &)
-  {
-    // Handle exceptions when the time source changes and initialize publish timestamp
-    previous_publish_timestamp_ = time;
-    should_publish = true;
-  }
-
-  if (should_publish)
-  {
-    if (realtime_odometry_publisher_)
+    catch (const std::runtime_error &)
     {
-      odometry_message_.header.stamp = time;
-      odometry_message_.pose.pose.position.x = odometry_.getX();
-      odometry_message_.pose.pose.position.y = odometry_.getY();
-      odometry_message_.pose.pose.orientation.x = orientation.x();
-      odometry_message_.pose.pose.orientation.y = orientation.y();
-      odometry_message_.pose.pose.orientation.z = orientation.z();
-      odometry_message_.pose.pose.orientation.w = orientation.w();
-      odometry_message_.twist.twist.linear.x = odometry_.getLinear();
-      odometry_message_.twist.twist.angular.z = odometry_.getAngular();
-      realtime_odometry_publisher_->try_publish(odometry_message_);
+      // Handle exceptions when the time source changes and initialize publish timestamp
+      previous_publish_timestamp_ = time;
+      should_publish = true;
     }
 
-    if (params_.enable_odom_tf && realtime_odometry_transform_publisher_)
+    if (should_publish)
     {
-      auto & transform = odometry_transform_message_.transforms.front();
-      transform.header.stamp = time;
-      transform.transform.translation.x = odometry_.getX();
-      transform.transform.translation.y = odometry_.getY();
-      transform.transform.rotation.x = orientation.x();
-      transform.transform.rotation.y = orientation.y();
-      transform.transform.rotation.z = orientation.z();
-      transform.transform.rotation.w = orientation.w();
-      realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_);
+      if (realtime_odometry_publisher_)
+      {
+        odometry_message_.header.stamp = time;
+        odometry_message_.pose.pose.position.x = odometry_.getX();
+        odometry_message_.pose.pose.position.y = odometry_.getY();
+        odometry_message_.pose.pose.orientation.x = orientation.x();
+        odometry_message_.pose.pose.orientation.y = orientation.y();
+        odometry_message_.pose.pose.orientation.z = orientation.z();
+        odometry_message_.pose.pose.orientation.w = orientation.w();
+        odometry_message_.twist.twist.linear.x = odometry_.getLinear();
+        odometry_message_.twist.twist.angular.z = odometry_.getAngular();
+        realtime_odometry_publisher_->try_publish(odometry_message_);
+      }
+
+      if (params_.enable_odom_tf && realtime_odometry_transform_publisher_)
+      {
+        auto & transform = odometry_transform_message_.transforms.front();
+        transform.header.stamp = time;
+        transform.transform.translation.x = odometry_.getX();
+        transform.transform.translation.y = odometry_.getY();
+        transform.transform.rotation.x = orientation.x();
+        transform.transform.rotation.y = orientation.y();
+        transform.transform.rotation.z = orientation.z();
+        transform.transform.rotation.w = orientation.w();
+        realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_);
+      }
     }
   }
 

diff_drive_controller/src/odometry.cpp

@@ -73,6 +73,25 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
   return true;
 }
 
+bool Odometry::update_from_pos(double left_pos, double right_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]:
+  double left_vel = (left_pos - left_wheel_old_pos_) / dt;
+  double right_vel = (right_pos - right_wheel_old_pos_) / dt;
+
+  // Update old position with current:
+  left_wheel_old_pos_ = left_pos;
+  right_wheel_old_pos_ = right_pos;
+
+  return update_from_vel(left_vel, right_vel, dt);
+}
+
 bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
 {
   const double dt = time.seconds() - timestamp_.seconds();
@@ -100,6 +119,26 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp
   return true;
 }
 
+bool Odometry::update_from_vel(double left_vel, double right_vel, double dt)
+{
+  // Compute linear and angular velocities of the robot:
+  const double linear_vel = (left_vel * left_wheel_radius_ + right_vel * right_wheel_radius_) * 0.5;
+  const double angular_vel =
+    (right_vel * right_wheel_radius_ - left_vel * left_wheel_radius_) / wheel_separation_;
+
+  // Integrate odometry:
+  integrate(linear_vel, angular_vel, dt);
+
+  // Estimate speeds using a rolling mean to filter them out:
+  linear_accumulator_.accumulate(linear_vel);
+  angular_accumulator_.accumulate(angular_vel);
+
+  linear_ = linear_accumulator_.getRollingMean();
+  angular_ = angular_accumulator_.getRollingMean();
+
+  return true;
+}
+
 void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time)
 {
   /// Save last linear and angular velocity:
@@ -112,6 +151,18 @@ void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time
   integrateExact(linear * dt, angular * dt);
 }
 
+bool Odometry::try_update_open_loop(double linear_vel, double angular_vel, double dt)
+{
+  // Integrate odometry:
+  integrate(linear_vel, angular_vel, dt);
+
+  // Save last linear and angular velocity:
+  linear_ = linear_vel;
+  angular_ = angular_vel;
+
+  return true;
+}
+
 void Odometry::resetOdometry()
 {
   x_ = 0.0;
@@ -161,6 +212,26 @@ void Odometry::integrateExact(double linear, double angular)
   }
 }
 
+void Odometry::integrate(double linear_vel, double angular_vel, double dt)
+{
+  const double dx = linear_vel * dt;
+  const double dheading = angular_vel * dt;
+  if (fabs(dheading) < 1e-6)
+  {
+    // For very small dheading, approximate to linear motion
+    x_ += (dx * std::cos(heading_));
+    y_ += (dx * std::sin(heading_));
+    heading_ += dheading;
+  }
+  else
+  {
+    const double heading_old = heading_;
+    heading_ += dheading;
+    x_ += ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old)));
+    y_ += -(dx / dheading) * (std::cos(heading_) - std::cos(heading_old));
+  }
+}
+
 void Odometry::resetAccumulators()
 {
   linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_);