From fe3a5683089d2c4fddbf1946380cab9f6a70e464 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 6 Aug 2025 19:45:12 +0530 Subject: [PATCH 1/7] Update odometry implementation in diff_drive --- .../diff_drive_controller/odometry.hpp | 20 ++++ .../src/diff_drive_controller.cpp | 93 ++++++++++--------- diff_drive_controller/src/odometry.cpp | 84 +++++++++++++++++ 3 files changed, 152 insertions(+), 45 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index ae4417a788..6458ae7e49 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/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 updateOpenLoop(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; #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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index cb4f2edcee..1b57e0a733 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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 --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index eb025ad23f..294e8fca11 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/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,30 @@ 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 + right_vel) * 0.5; + const double angular_vel = (right_vel - left_vel) / 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 +161,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 +227,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_ = x_ + (dx * std::cos(heading_)); + y_ = y_ + (dx * std::sin(heading_)); + heading_ = heading_ + dheading; + } + else + { + const double heading_old = heading_; + heading_ = heading_ + dheading; + x_ = x_ + ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old))); + y_ = y_ - (dx / dheading) * (std::cos(heading_) - std::cos(heading_old)); + } +} + void Odometry::resetAccumulators() { linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); From 13edbec7a0ba612541098e75f5098349a710a183 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 20 Aug 2025 19:03:44 +0530 Subject: [PATCH 2/7] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../include/diff_drive_controller/odometry.hpp | 14 +++++++------- diff_drive_controller/src/odometry.cpp | 12 ++++++------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 6458ae7e49..70a29346f2 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -40,23 +40,23 @@ 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 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 " + "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 updateOpenLoop(const double & linear_vel, const double & angular_vel, const " + "Replaced by bool updateOpenLoop(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 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); + const double linear_vel, const double angular_vel, const rclcpp::Time & time); void resetOdometry(); double getX() const { return x_; } @@ -81,7 +81,7 @@ class Odometry [[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 integrate(const double dx, const double dheading); void resetAccumulators(); // Current timestamp: diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 294e8fca11..a3a96bb245 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -232,16 +232,16 @@ void Odometry::integrate(const double & dx, const double & dheading) if (fabs(dheading) < 1e-6) { // For very small dheading, approximate to linear motion - x_ = x_ + (dx * std::cos(heading_)); - y_ = y_ + (dx * std::sin(heading_)); - heading_ = heading_ + dheading; + x_ += (dx * std::cos(heading_)); + y_ += (dx * std::sin(heading_)); + heading_ += dheading; } else { const double heading_old = heading_; - heading_ = heading_ + dheading; - x_ = x_ + ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old))); - y_ = y_ - (dx / dheading) * (std::cos(heading_) - std::cos(heading_old)); + heading_ += dheading; + x_ += ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old))); + y_ += -(dx / dheading) * (std::cos(heading_) - std::cos(heading_old)); } } From 629cf5efd393657112d120f8860a16ff0a8c3d65 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 20 Aug 2025 19:05:22 +0530 Subject: [PATCH 3/7] Fix deprecation warning --- .../include/diff_drive_controller/odometry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 70a29346f2..21236688d0 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -48,7 +48,7 @@ class Odometry "rclcpp::Time & time).")]] bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); [[deprecated( - "Replaced by bool updateOpenLoop(const double linear_vel, const double angular_vel, const " + "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); From d387750bff2973b112223ee21cf7b3335eeca5b5 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 20 Aug 2025 19:08:20 +0530 Subject: [PATCH 4/7] Multiply by wheel radius in updateFromVel --- diff_drive_controller/src/odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index a3a96bb245..4d74dcbd0b 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -131,8 +131,8 @@ bool Odometry::updateFromVel( const double dt = time.seconds() - timestamp_.seconds(); // Compute linear and angular velocities of the robot: - const double linear_vel = (left_vel + right_vel) * 0.5; - const double angular_vel = (right_vel - left_vel) / wheel_separation_; + 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); From bd1283b149ab9befc89ad8068061474ec25364c1 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 20 Aug 2025 19:11:56 +0530 Subject: [PATCH 5/7] Fix pre-commit --- diff_drive_controller/src/odometry.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 4d74dcbd0b..46781cb18c 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -132,7 +132,8 @@ bool Odometry::updateFromVel( // 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_; + 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); From 93806d62f1c878bca5d0dbe6a32ec05a0965d1ee Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 4 Sep 2025 09:49:36 +0530 Subject: [PATCH 6/7] Fix build errors --- diff_drive_controller/src/odometry.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 46781cb18c..ef7081c7ec 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -74,7 +74,7 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti } bool Odometry::updateFromPos( - const double & left_pos, const double & right_pos, const rclcpp::Time & time) + 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 @@ -126,13 +126,13 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp } bool Odometry::updateFromVel( - const double & left_vel, const double & right_vel, const rclcpp::Time & time) + 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 = + 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: @@ -163,7 +163,7 @@ void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time } bool Odometry::tryUpdateOpenLoop( - const double & linear_vel, const double & angular_vel, const rclcpp::Time & time) + const double linear_vel, const double angular_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); @@ -228,7 +228,7 @@ void Odometry::integrateExact(double linear, double angular) } } -void Odometry::integrate(const double & dx, const double & dheading) +void Odometry::integrate(const double dx, const double dheading) { if (fabs(dheading) < 1e-6) { From 1d47fed68734a9b3aa4f412becf47512f0efbc87 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Tue, 9 Sep 2025 15:08:16 +0530 Subject: [PATCH 7/7] Update diff_drive_controller/src/odometry.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- diff_drive_controller/src/odometry.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index ef7081c7ec..41a00ac698 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -91,11 +91,7 @@ bool Odometry::updateFromPos( left_wheel_old_pos_ = left_pos; right_wheel_old_pos_ = right_pos; - if (updateFromVel(left_vel, right_vel, time)) - { - return true; - } - return false; + return updateFromVel(left_vel, right_vel, time); } bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)