feat(autoware_trajectory_optimizer): fix acceleration recalculation after speed cropping #11686
Conversation
Signed-off-by: Daniel <[email protected]>
Signed-off-by: Daniel <[email protected]>
github-actions
bot
added
the
component:planning
|
Thank you for contributing to the Autoware project! 🚧 If your pull request is in progress, switch it to draft mode. Please ensure:
|
danielsanchezaran
added
run:build-and-test-differential
danielsanchezaran
requested review from
SakodaShintaro,
go-sakayori and
yukkysaito
as code owners
There was a problem hiding this comment.
Pull Request Overview
This PR fixes a bug in the acceleration recalculation logic when velocity is capped to a maximum value. The previous implementation recalculated acceleration for all points indiscriminately, causing noisy acceleration profiles. The new implementation intelligently identifies continuous segments of points that exceed the velocity limit, caps only those velocities, and recalculates acceleration only for affected boundary points using preserved time delta (dt) information.
Key Changes:
- Replaces simple velocity capping with segment-aware velocity limiting
- Preserves original dt values before modifying velocities to ensure accurate acceleration recalculation
- Adds comprehensive test coverage for various segment configurations (interior, boundary, single-point, multiple segments)
Reviewed Changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.
| File | Description |
|---|---|
planning/autoware_trajectory_optimizer/src/utils.cpp |
Implements new segment-based velocity capping algorithm with dt preservation and selective acceleration recalculation |
planning/autoware_trajectory_optimizer/tests/test_trajectory_optimizer.cpp |
Adds 10 new test cases covering edge cases and segment configurations; includes vector header for segment tracking |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| const auto acc = static_cast<double>(from.acceleration_mps2); | ||
| if (std::abs(acc) < 1e-6) { | ||
| // Avoid division by zero; assume a small acceleration | ||
| return std::abs(dv) / 1e-6; |
There was a problem hiding this comment.
The fallback value 1e-6 is a magic number. Consider defining it as a named constant (e.g., MIN_ACCELERATION_THRESHOLD) to improve code readability and maintainability.
| const auto dv = static_cast<double>(to.longitudinal_velocity_mps) - | ||
| static_cast<double>(from.longitudinal_velocity_mps); | ||
| const auto acc = static_cast<double>(from.acceleration_mps2); | ||
| if (std::abs(acc) < 1e-6) { |
There was a problem hiding this comment.
The threshold 1e-6 for detecting near-zero acceleration is a magic number. Consider defining it as a named constant (e.g., EPSILON_ACCELERATION) for better maintainability and to make the threshold value explicit and adjustable.
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## main #11686 +/- ##
==========================================
+ Coverage 17.80% 17.88% +0.08%
==========================================
Files 1746 1747 +1
Lines 122013 122390 +377
Branches 42794 42962 +168
==========================================
+ Hits 21723 21889 +166
- Misses 82076 82289 +213
+ Partials 18214 18212 -2
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. 🚀 New features to boost your workflow:
|
1 participant
Description
This PR fixes faulty recalculation of acceleration by first preserving initial dt information between points, capping the speed of points that are faster than our limit, then using the previously stored dt information to recalculate the acceleration of the affected points only.
The new implementation checks for segments of continuous points that will have their velocity changed and only modifies said points and points before and after the segments:
Before:
The acceleration (light green plot) becomes very noisy and spiky, even for points that dont have their speeds changed:
Screencast.from.2025.11.21.10.08.04.webm
After:
Acceleration does not become noisy when using the velocity limiter and it is smoother, even when setting the max speed to be 5.5 m/s
Screencast.from.11-21-2025.01.37.58.PM.webm
Related links
Parent Issue:
How was this PR tested?
PSim
test.webm
Notes for reviewers
None.
Interface changes
None.
Effects on system behavior
None.