Apply suggestions from code review

Co-authored-by: Julia Jia <[email protected]>

Author: Amronos

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -40,17 +40,16 @@ class Odometry
   [[deprecated]]
   void init(const rclcpp::Time & time);
   [[deprecated(
-    "Replaced by bool update_from_pos(double left_pos, double right_pos, const "
-    "rclcpp::Time & time).")]]
+    "Replaced by bool update_from_pos(double left_pos, double right_pos, double "
+    "dt).")]]
   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).")]]
+ [[deprecated(
+    "Replaced by bool update_from_vel(double left_vel, double right_vel, double "
+    "dt).")]]
   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).")]]
+       "Replaced by bool try_update_open_loop(double linear_vel, double angular_vel, double "
+    "dt).")]]
   void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);
 
   bool update_from_pos(double left_pos, double right_pos, double dt);

diff_drive_controller/src/diff_drive_controller.cpp

@@ -221,7 +221,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
     bool should_publish = false;
     try
     {
-      if (previous_publish_timestamp_ + publish_period_ < time)
+      if (previous_publish_timestamp_ + publish_period_ <= time)
       {
         previous_publish_timestamp_ += publish_period_;
         should_publish = true;

diff_drive_controller/src/odometry.cpp

@@ -121,6 +121,10 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp
 
 bool Odometry::update_from_vel(double left_vel, double right_vel, double dt)
 {
+  if (std::fabs(dt) < 1e-6)
+  {
+    return false;  // Interval too small to integrate with
+  }
   // 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 =
@@ -214,6 +218,11 @@ void Odometry::integrateExact(double linear, double angular)
 
 void Odometry::integrate(double linear_vel, double angular_vel, double dt)
 {
+  // Skip integration for invalid time intervals
+  if (std::fabs(dt) < 1e-6)
+  {
+    return;
+  }
   const double dx = linear_vel * dt;
   const double dheading = angular_vel * dt;
   if (fabs(dheading) < 1e-6)