From 14053a78e7979ebe9998ef0261201531a69e06b9 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 27 Nov 2024 02:35:52 +0000 Subject: [PATCH] Updated diff_drive odometry class to consistantly use velocity where it claims to, and pass timestep in to every method that makes use of it. This was requested in #271. - The result is that the class is very slightly less efficient (but still quite trivial) due to passing around a extra double. - The other result is that it is no longer possible for velocity based odometry updates calculated incorrectly when the timestep fails to keep up. To prevent position based updates from now suffering the inverse of the velocity timestep/dt mismatch, the measured delta is used exclusively for position based updates (retaining the existing behavior), while the configured timestep is used for open and closed loop velocity updates. I also added a 1s throttled warning if there is a major mismatch between measured and configured timestep (+/-20%) since this would cause a slight accumulation of integration errors on curved paths (I believe), depending on velocity, curvature and mismatch size. Similar requested period/measured period mismatches may be a nuisance for open loop (which is velocity based) and closed loop velocity based updates, but the impact is likely negligible (in this class). --- .../diff_drive_controller/odometry.hpp | 10 ++--- .../src/diff_drive_controller.cpp | 24 ++++++++-- diff_drive_controller/src/odometry.cpp | 44 +++++++++---------- 3 files changed, 45 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index edca431d3d..6cf2f4ed80 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -40,9 +40,9 @@ class Odometry explicit Odometry(size_t velocity_rolling_window_size = 10); void init(const rclcpp::Time & time); - bool update(double left_pos, double right_pos, const rclcpp::Time & time); - bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); - void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + bool updateFromPosition(double left_pos, double right_pos, const rclcpp::Time & time, const double dt); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time, const double dt); + void updateFromVelocityOpenLoop(double linear, double angular, const rclcpp::Time & time, const double dt); void resetOdometry(); double getX() const { return x_; } @@ -62,8 +62,8 @@ class Odometry using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; #endif - void integrateRungeKutta2(double linear, double angular); - void integrateExact(double linear, double angular); + void integrateRungeKutta2(double linear, double angular, const double dt); + void integrateExact(double linear, double angular, const double dt); 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 66da6d6738..0eb9ca27b7 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -141,9 +141,10 @@ controller_interface::return_type DiffDriveController::update( 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; + if (params_.open_loop) { - odometry_.updateOpenLoop(linear_command, angular_command, time); + odometry_.updateFromVelocityOpenLoop(linear_command, angular_command, time, period.seconds()); } else { @@ -171,13 +172,28 @@ controller_interface::return_type DiffDriveController::update( if (params_.position_feedback) { - odometry_.update(left_feedback_mean, right_feedback_mean, time); + // Warn if actual timestep doesn't match period, + // since + const double measured_period = ( + get_node()->get_clock()->now() - previous_update_timestamp_).seconds(); + + // +/- 20% of nominal loop time as a starting point to warn wheel encoder + // users that the real update rate doesn't match the requested update period + if ( std::fabs(period.seconds()-measured_period) > period.seconds()/5.0 ) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "Integrating odometry at measured delta of %.3fs, but desired update period is %.3fs", + measured_period, period.seconds()); + } + + // update odometry + odometry_.updateFromPosition(left_feedback_mean, right_feedback_mean, time, measured_period); } else { odometry_.updateFromVelocity( - left_feedback_mean * left_wheel_radius * period.seconds(), - right_feedback_mean * right_wheel_radius * period.seconds(), time); + left_feedback_mean * left_wheel_radius, + right_feedback_mean * right_wheel_radius, time, period.seconds()); } } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 99bb22890d..dc89ba2433 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -45,10 +45,9 @@ void Odometry::init(const rclcpp::Time & time) timestamp_ = time; } -bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & time) +bool Odometry::updateFromPosition(double left_pos, double right_pos, const rclcpp::Time & time, const double dt) { // We cannot estimate the speed with very small time intervals: - const double dt = time.seconds() - timestamp_.seconds(); if (dt < 0.0001) { return false; // Interval too small to integrate with @@ -59,35 +58,33 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti const double right_wheel_cur_pos = right_pos * right_wheel_radius_; // Estimate velocity of wheels using old and current position: - const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + const double left_wheel_est_vel = (left_wheel_cur_pos - left_wheel_old_pos_)/dt; + const double right_wheel_est_vel = (right_wheel_cur_pos - right_wheel_old_pos_)/dt; // Update old position with current: left_wheel_old_pos_ = left_wheel_cur_pos; right_wheel_old_pos_ = right_wheel_cur_pos; - updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time); + updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time, dt); return true; } -bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) +bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time, const double dt) { - const double dt = time.seconds() - timestamp_.seconds(); - // Compute linear and angular diff: const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity const double angular = (right_vel - left_vel) / wheel_separation_; // Integrate odometry: - integrateExact(linear, angular); + integrateExact(linear, angular, dt); timestamp_ = time; // Estimate speeds using a rolling mean to filter them out: - linear_accumulator_.accumulate(linear / dt); - angular_accumulator_.accumulate(angular / dt); + linear_accumulator_.accumulate(linear); + angular_accumulator_.accumulate(angular); linear_ = linear_accumulator_.getRollingMean(); angular_ = angular_accumulator_.getRollingMean(); @@ -95,16 +92,15 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp return true; } -void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time) +void Odometry::updateFromVelocityOpenLoop(double linear, double angular, const rclcpp::Time & time, const double dt) { /// Save last linear and angular velocity: linear_ = linear; angular_ = angular; /// Integrate odometry: - const double dt = time.seconds() - timestamp_.seconds(); timestamp_ = time; - integrateExact(linear * dt, angular * dt); + integrateExact(linear, angular, dt); } void Odometry::resetOdometry() @@ -129,28 +125,28 @@ void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) resetAccumulators(); } -void Odometry::integrateRungeKutta2(double linear, double angular) +void Odometry::integrateRungeKutta2(double linear, double angular, const double dt) { - const double direction = heading_ + angular * 0.5; + const double direction = heading_ + angular * 0.5 * dt; /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; + x_ += linear * cos(direction) * dt; + y_ += linear * sin(direction) * dt; + heading_ += angular * dt; } -void Odometry::integrateExact(double linear, double angular) +void Odometry::integrateExact(double linear, double angular, const double dt) { - if (fabs(angular) < 1e-6) + if (fabs(angular*dt) < 1e-6) { - integrateRungeKutta2(linear, angular); + integrateRungeKutta2(linear, angular, dt); } else { /// Exact integration (should solve problems when angular is zero): const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; + const double r = linear / angular * dt; + heading_ += angular * dt; x_ += r * (sin(heading_) - sin(heading_old)); y_ += -r * (cos(heading_) - cos(heading_old)); }