Skip to content

[velocity_smoother] Fix accel and deccel inverted for negative speeds #3529

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
SteveMacenski merged 13 commits into from
Apr 18, 2023
+373 −15
Merged

[velocity_smoother] Fix accel and deccel inverted for negative speeds #3529

Show file tree
Hide file tree
Changes from 12 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
439a86c
fix inverted accel / deccel
Apr 7, 2023
cd3020e
handle speed through 0.0
Apr 11, 2023
a45481b
add applyConstraints tests
Apr 12, 2023
10d8673
fold logic
Apr 12, 2023
228a53b
same logic in findEtaConstraint
Apr 14, 2023
ec020c5
lint
Apr 14, 2023
2bc6a16
Update nav2_velocity_smoother/src/velocity_smoother.cpp
SteveMacenski Apr 14, 2023
da80677
Update nav2_velocity_smoother/src/velocity_smoother.cpp
SteveMacenski Apr 14, 2023
db88df2
findEtaConstraint tests
Apr 14, 2023
0b64a72
space
Apr 14, 2023
c984e69
lint
Apr 14, 2023
bbeabd6
typos
Apr 17, 2023
8e4a622
comment typos
Apr 18, 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
35 changes: 30 additions & 5 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 @@ -182,9 +182,21 @@ double VelocitySmoother::findEtaConstraint(
const double v_curr, const double v_cmd, const double accel, const double decel)
{
// Exploiting vector scaling properties
const double v_component_max = accel / smoothing_frequency_;
const double v_component_min = decel / smoothing_frequency_;
const double dv = v_cmd - v_curr;
double dv = v_cmd - v_curr;

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 (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
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_;
}

if (dv > v_component_max) {
return v_component_max / dv;
Expand All @@ -202,8 +214,21 @@ 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 (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
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_;
}

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

Expand Down
Loading