Update odometry implementation in diff_drive

diff_drive_controller/include/diff_drive_controller/odometry.hpp

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

diff_drive_controller/src/diff_drive_controller.cpp

@@ -160,9 +160,11 @@ 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_.tryUpdateOpenLoop(linear_command, angular_command, time);
   }
   else
   {
@@ -200,63 +202,64 @@ 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_.updateFromPos(left_feedback_mean, right_feedback_mean, time);
     }
     else
     {
-      odometry_.updateFromVelocity(
-        left_feedback_mean * left_wheel_radius * period.seconds(),
-        right_feedback_mean * right_wheel_radius * period.seconds(), time);
+      odometry_updated = odometry_.updateFromVel(left_feedback_mean, right_feedback_mean, time);
     }
   }
 
-  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_->trylock())
+    catch (const std::runtime_error &)
     {
-      auto & odometry_message = realtime_odometry_publisher_->msg_;
-      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_->unlockAndPublish();
+      // 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_->trylock())
+    if (should_publish)
     {
-      auto & transform = realtime_odometry_transform_publisher_->msg_.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_->unlockAndPublish();
+      if (realtime_odometry_publisher_->trylock())
+      {
+        auto & odometry_message = realtime_odometry_publisher_->msg_;
+        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_->unlockAndPublish();
+      }
+
+      if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
+      {
+        auto & transform = realtime_odometry_transform_publisher_->msg_.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_->unlockAndPublish();
+      }
     }
   }
 

diff_drive_controller/src/odometry.cpp

@@ -73,6 +73,31 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
   return true;
 }
 
+bool Odometry::updateFromPos(
+  const double left_pos, const double right_pos, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();
+  // 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;
+
+  if (updateFromVel(left_vel, right_vel, time))
+  {
+    return true;
+  }
+  return false;
+}
+
 bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
 {
   const double dt = time.seconds() - timestamp_.seconds();
@@ -100,6 +125,31 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp
   return true;
 }
 
+bool Odometry::updateFromVel(
+  const double left_vel, const double right_vel, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();
+
+  // 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 * dt, angular_vel * dt);
+
+  timestamp_ = time;
+
+  // 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 +162,23 @@ void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time
   integrateExact(linear * dt, angular * dt);
 }
 
+bool Odometry::tryUpdateOpenLoop(
+  const double linear_vel, const double angular_vel, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();
+
+  // Integrate odometry:
+  integrate(linear_vel * dt, angular_vel * dt);
+
+  timestamp_ = time;
+
+  // Save last linear and angular velocity:
+  linear_ = linear_vel;
+  angular_ = angular_vel;
+
+  return true;
+}
+
 void Odometry::resetOdometry()
 {
   x_ = 0.0;
@@ -161,6 +228,24 @@ void Odometry::integrateExact(double linear, double angular)
   }
 }
 
+void Odometry::integrate(const double dx, const double dheading)
+{
+  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_);