Apply suggestions from code review

Author: Amronos

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -40,22 +40,22 @@ class Odometry
   [[deprecated]]
   void init(const rclcpp::Time & time);
   [[deprecated(
-    "Replaced by bool updateFromPos(const double left_pos, const double right_pos, const "
+    "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 updateFromVel(const double left_vel, const double right_vel, const "
+    "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 tryUpdateOpenLoop(const double linear_vel, const double angular_vel, const "
+    "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 updateFromPos(const double left_pos, const double right_pos, const double dt);
-  bool updateFromVel(const double left_vel, const double right_vel, const double dt);
-  bool tryUpdateOpenLoop(const double linear_vel, const double angular_vel, const double dt);
+  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_; }
@@ -75,12 +75,12 @@ class Odometry
   using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
 #endif
 
-  [[deprecated("Replaced by void integrate(const double & dx, const double & dheading).")]]
+  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
   void integrateRungeKutta2(double linear, double angular);
-  [[deprecated("Replaced by void integrate(const double & dx, const double & dheading).")]]
+  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
   void integrateExact(double linear, double angular);
 
-  void integrate(const double dx, const double dheading);
+  void integrate(double linear_vel, double angular_vel, double dt);
   void resetAccumulators();
 
   // Current timestamp:

diff_drive_controller/src/diff_drive_controller.cpp

@@ -165,7 +165,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   if (params_.open_loop)
   {
     odometry_updated =
-      odometry_.tryUpdateOpenLoop(linear_command, angular_command, period.seconds());
+      odometry_.try_update_open_loop(linear_command, angular_command, period.seconds());
   }
   else
   {
@@ -204,12 +204,12 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
     if (params_.position_feedback)
     {
       odometry_updated =
-        odometry_.updateFromPos(left_feedback_mean, right_feedback_mean, period.seconds());
+        odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds());
     }
     else
     {
       odometry_updated =
-        odometry_.updateFromVel(left_feedback_mean, right_feedback_mean, period.seconds());
+        odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds());
     }
   }
 

diff_drive_controller/src/odometry.cpp

@@ -73,7 +73,7 @@ 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 double dt)
+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)
@@ -89,7 +89,7 @@ bool Odometry::updateFromPos(const double left_pos, const double right_pos, cons
   left_wheel_old_pos_ = left_pos;
   right_wheel_old_pos_ = right_pos;
 
-  return updateFromVel(left_vel, right_vel, dt);
+  return update_from_vel(left_vel, right_vel, dt);
 }
 
 bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
@@ -119,15 +119,15 @@ 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 double dt)
+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 * dt, angular_vel * dt);
+  integrate(linear_vel, angular_vel, dt);
 
   // Estimate speeds using a rolling mean to filter them out:
   linear_accumulator_.accumulate(linear_vel);
@@ -151,10 +151,10 @@ 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 double dt)
+bool Odometry::try_update_open_loop(double linear_vel, double angular_vel, double dt)
 {
   // Integrate odometry:
-  integrate(linear_vel * dt, angular_vel * dt);
+  integrate(linear_vel, angular_vel, dt);
 
   // Save last linear and angular velocity:
   linear_ = linear_vel;
@@ -212,8 +212,10 @@ void Odometry::integrateExact(double linear, double angular)
   }
 }
 
-void Odometry::integrate(const double dx, const double dheading)
+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