Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve diff_drive_controller odometry velocity and timestep handling #1394

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+45 −33
Open

Improve diff_drive_controller odometry velocity and timestep handling #1394

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
14053a7
Updated diff_drive odometry class to consistantly
josephduchesne Nov 27, 2024
b8e2305
Merge branch 'master' into master
josephduchesne Nov 28, 2024
8121ed2
Merge branch 'master' into master
christophfroehlich Nov 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions diff_drive_controller/include/diff_drive_controller/odometry.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_; }
Expand All @@ -62,8 +62,8 @@ class Odometry
using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
#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:
Expand Down
24 changes: 20 additions & 4 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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 = (
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why should the measured period be different from the period argument of the update method? This is calculated from the controller_manager, we can assume that this is always valid?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was thinking on this some more myself and came to the same conclusion.

I'm hoping to rework this PR sometime next week focusing on the code quality/cleanup aspects suggested here and remove the inconsistent time handling (which as you suggest is probably a distinction without a difference).

Recently I've switched to using the RT_Linux kernel and set up permissions for real-time and found a fairly dramatic difference in perceived responsiveness with it enabled in the diff drive controller. Something isn't handling time/dates/update timesteps properly and I think I incorrectly identified this as the culprit. It still might be, but I should have started off with unit tests (or at least minimum steps to reproduce) rather than complicating an otherwise straightforward code cleanup.

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

Expand Down
44 changes: 20 additions & 24 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -59,52 +58,49 @@ 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;
Copy link
Contributor

@christophfroehlich christophfroehlich Nov 30, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

timestamp_ class variable should be removed now (marked as deprecated to be removed with the old methods)

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

good catch


// 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();

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()
Expand All @@ -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));
}
Expand Down
Loading