From fe3a5683089d2c4fddbf1946380cab9f6a70e464 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 6 Aug 2025 19:45:12 +0530 Subject: [PATCH 01/13] 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 02/13] 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 03/13] 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 04/13] 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 05/13] 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 06/13] 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 07/13] 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) From 77eade3a2f89d51392a52d7755dffb8ea1ac56cf Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 24 Sep 2025 11:12:54 +0530 Subject: [PATCH 08/13] Apply suggestions from code review --- .../diff_drive_controller/odometry.hpp | 7 +++--- .../src/diff_drive_controller.cpp | 23 ++++++++++--------- diff_drive_controller/src/odometry.cpp | 20 ++++------------ 3 files changed, 19 insertions(+), 31 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 21236688d0..31593d95c4 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -53,10 +53,9 @@ class Odometry "& 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); + 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); void resetOdometry(); double getX() const { return x_; } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1b57e0a733..720503462f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -164,7 +164,8 @@ controller_interface::return_type DiffDriveController::update_and_write_commands bool odometry_updated = false; if (params_.open_loop) { - odometry_updated = odometry_.tryUpdateOpenLoop(linear_command, angular_command, time); + odometry_updated = + odometry_.tryUpdateOpenLoop(linear_command, angular_command, period.seconds()); } else { @@ -202,11 +203,13 @@ controller_interface::return_type DiffDriveController::update_and_write_commands if (params_.position_feedback) { - odometry_updated = odometry_.updateFromPos(left_feedback_mean, right_feedback_mean, time); + odometry_updated = + odometry_.updateFromPos(left_feedback_mean, right_feedback_mean, period.seconds()); } else { - odometry_updated = odometry_.updateFromVel(left_feedback_mean, right_feedback_mean, time); + odometry_updated = + odometry_.updateFromVel(left_feedback_mean, right_feedback_mean, period.seconds()); } } @@ -664,15 +667,13 @@ DiffDriveController::on_export_reference_interfaces() std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back( - hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); - reference_interfaces.push_back( - hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); return reference_interfaces; } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 41a00ac698..143a2dce6f 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -73,10 +73,8 @@ 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) +bool Odometry::updateFromPos(const double left_pos, const double right_pos, const double dt) { - const double dt = time.seconds() - timestamp_.seconds(); // We cannot estimate angular velocity with very small time intervals if (std::fabs(dt) < 1e-6) { @@ -91,7 +89,7 @@ bool Odometry::updateFromPos( left_wheel_old_pos_ = left_pos; right_wheel_old_pos_ = right_pos; - return updateFromVel(left_vel, right_vel, time); + return updateFromVel(left_vel, right_vel, dt); } bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) @@ -121,11 +119,8 @@ 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) +bool Odometry::updateFromVel(const double left_vel, const double right_vel, const double dt) { - 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 = @@ -134,8 +129,6 @@ bool Odometry::updateFromVel( // 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); @@ -158,16 +151,11 @@ 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) +bool Odometry::tryUpdateOpenLoop(const double linear_vel, const double angular_vel, const double dt) { - 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; From a95a450e9167628c5da53ac0611cbc7672f7ef96 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 24 Sep 2025 11:26:01 +0530 Subject: [PATCH 09/13] Fix pre-commit --- .../src/diff_drive_controller.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 720503462f..2dc95d0333 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -667,13 +667,15 @@ DiffDriveController::on_export_reference_interfaces() std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); return reference_interfaces; } From 8ecb548fcf744c76bef0795b59de6eec406ddc5c Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 24 Sep 2025 22:00:00 +0530 Subject: [PATCH 10/13] Fix pre-commit --- .../src/diff_drive_controller.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2dc95d0333..720503462f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -667,15 +667,13 @@ DiffDriveController::on_export_reference_interfaces() std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back( - hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - - reference_interfaces.push_back( - hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); return reference_interfaces; } From ec254734cd78179d208fcd66c13828ee079b2f29 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Fri, 3 Oct 2025 18:33:41 +0000 Subject: [PATCH 11/13] Fix pre-commit --- .../src/diff_drive_controller.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 720503462f..feb4e2e7e2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -667,13 +667,15 @@ DiffDriveController::on_export_reference_interfaces() std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); return reference_interfaces; } From 574c3bae6ec7157bdfb9529b27b2c036d883cdfc Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 22 Oct 2025 06:49:02 +0530 Subject: [PATCH 12/13] Update diff_drive_controller/src/diff_drive_controller.cpp --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 699b086c35..a0d15f1f20 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -250,7 +250,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands realtime_odometry_publisher_->try_publish(odometry_message_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_ { auto & transform = odometry_transform_message_.transforms.front(); transform.header.stamp = time; From f52b6b1d225d787270d8223d545c1e00679f056e Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 22 Oct 2025 06:53:54 +0530 Subject: [PATCH 13/13] Update diff_drive_controller/src/diff_drive_controller.cpp --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a0d15f1f20..1829657f6f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -250,7 +250,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands realtime_odometry_publisher_->try_publish(odometry_message_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_ + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { auto & transform = odometry_transform_message_.transforms.front(); transform.header.stamp = time;