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)); }