Skip to content

AUTO-763 fix smoother accel decel #19

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

Merged
doisyg merged 2 commits into from
Apr 11, 2023
Merged

AUTO-763 fix smoother accel decel #19

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4c51a39
fix inverted accel / deccel
Apr 7, 2023
6d448d1
handle speed through 0.0
Apr 11, 2023
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
22 changes: 20 additions & 2 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,8 +202,26 @@ double VelocitySmoother::applyConstraints(
const double accel, const double decel, const double eta)
{
double dv = v_cmd - v_curr;
const double v_component_max = accel / smoothing_frequency_;
const double v_component_min = decel / smoothing_frequency_;

double v_component_max;
double v_component_min;

// Accelerating if magnitude of v_cmd is above magnitude of v_curr
// and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
// Deccelerating otherwise
if (v_curr * v_cmd >= 0.0) {
if (abs(v_cmd) >= abs(v_curr)) {
v_component_max = accel / smoothing_frequency_;
v_component_min = -accel / smoothing_frequency_;
} else {
v_component_max = -decel / smoothing_frequency_;
v_component_min = decel / smoothing_frequency_;
}
} else {
v_component_max = -decel / smoothing_frequency_;
v_component_min = decel / smoothing_frequency_;
}
Comment on lines +206 to +223
Copy link

@scheunemann scheunemann Apr 11, 2023

Choose a reason for hiding this comment

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

Suggested change
double v_component_max;
double v_component_min;
// Accelerating if magnitude of v_cmd is above magnitude of v_curr
// and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
// Deccelerating otherwise
if (v_curr * v_cmd >= 0.0) {
if (abs(v_cmd) >= abs(v_curr)) {
v_component_max = accel / smoothing_frequency_;
v_component_min = -accel / smoothing_frequency_;
} else {
v_component_max = -decel / smoothing_frequency_;
v_component_min = decel / smoothing_frequency_;
}
} else {
v_component_max = -decel / smoothing_frequency_;
v_component_min = decel / smoothing_frequency_;
}
double v_component_max = -decel / smoothing_frequency_;
double v_component_min = decel / smoothing_frequency_;
// Accelerating if magnitude of v_cmd is above magnitude of v_curr
// and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
// Deccelerating otherwise
if (v_curr * v_cmd >= 0.0 && abs(v_cmd) >= abs(v_curr)) {
v_component_max = accel / smoothing_frequency_;
v_component_min = -accel / smoothing_frequency_;
}

Above reads a little better for me as it shows there is one well defined case to ever accelerate at a glance.

Copy link
Author

@doisyg doisyg Apr 11, 2023

Choose a reason for hiding this comment

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

Reads better but less efficient, v_component_max / v_component_min are computed twice ~50% of the time


return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);
}

Expand Down