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

Jump to bottom

refactor(motion_velocity_smoother): change curvature calculation #1638

Merged
merged 6 commits into from
Sep 2, 2022
Merged

refactor(motion_velocity_smoother): change curvature calculation #1638

merged 6 commits into from
Sep 2, 2022

Conversation

brkay54
Copy link
Member

@brkay54 brkay54 commented Aug 19, 2022 •
edited by TakaHoribe
Loading

Signed-off-by: Berkay berkay@leodrive.ai

Description

Related: #1071 (comment)

While curvature is calculating, the function copied first and last curvature points (that can not calculate due to distance) from next or before points that can be calculated so it causes curvature with high error.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

@brkay54 brkay54 self-assigned this Aug 19, 2022
@brkay54 brkay54 force-pushed the update-curvature-calculation branch 2 times, most recently from a93a2c6 to d091ddc Compare August 19, 2022 12:21
@brkay54 brkay54 changed the title refactor(trajectory_utils): change curvature calculation refactor(motion_velocity_smoother): change curvature calculation Aug 19, 2022
@codecov
Copy link

codecov bot commented Aug 19, 2022 •
edited
Loading

Codecov Report

Merging #1638 (8c2080c) into main (8c2080c) will not change coverage.
The diff coverage is n/a.

❗ Current head 8c2080c differs from pull request most recent head a786a62. Consider uploading reports for the commit a786a62 to get more accurate results

@@           Coverage Diff           @@
##             main    #1638   +/-   ##
=======================================
  Coverage   10.38%   10.38%           
=======================================
  Files        1212     1212           
  Lines       87402    87402           
  Branches    20274    20274           
=======================================
  Hits         9081     9081           
  Misses      68842    68842           
  Partials     9479     9479
Flag Coverage Δ
total 10.37% <0.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

TakaHoribe
TakaHoribe reviewed Aug 23, 2022
View reviewed changes
Copy link
Contributor

@TakaHoribe TakaHoribe left a comment

Choose a reason for hiding this comment

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

@brkay54 Thank you for your contribution.
I confirmed it works well in my simulation.

Since it has a long backward path length, we can not see a big difference, but definitely better.
(blue: closest curvature of smoother node in current implementation, red: with this PR)
image
image

planning/motion_velocity_smoother/src/trajectory_utils.cpp Outdated
const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
const auto p1 = getPoint(trajectory.at(i));
const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
k_arr.push_back(calcCurvature(p0, p1, p2));
Copy link
Contributor

Choose a reason for hiding this comment

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

The calcCurvature() in autoware_utils lib throws an exception when these points are too close. Would you add special handling for closed points? Maybe use previous curvature or something.
(Or just keep using try-catch. It is not a nice way though).

Copy link
Member Author

Choose a reason for hiding this comment

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

I added the error handler, if there is a previous curvature, it push back. Else it push back 0.0. Is it proper way to do?

Copy link
Contributor

Choose a reason for hiding this comment

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

LGTM. Thank you!

TakaHoribe
TakaHoribe reviewed Aug 29, 2022
View reviewed changes
planning/motion_velocity_smoother/src/trajectory_utils.cpp Outdated
@@ -212,22 +212,31 @@ std::vector<double> calcTrajectoryCurvatureFrom3Points(

// calculate curvature by circle fitting from three points
std::vector<double> k_arr;
for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) {
// for first point curvature = 0;
k_arr.push_back(0.0);
Copy link
Contributor

Choose a reason for hiding this comment

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

@brkay54 For the first and last points, how about taking a copy from the next/previous value?

std::vector<double> k_arr(trajectory.size(), 0.0);

...
// compute k for each index
...

k_arr.at(0) = k_arr.at(1);
k_arr.back() = k_arr.at(trajectory.size() - 2);

@TakaHoribe TakaHoribe self-requested a review August 29, 2022 06:29
TakaHoribe
TakaHoribe approved these changes Sep 2, 2022
View reviewed changes
Copy link
Contributor

@TakaHoribe TakaHoribe left a comment

Choose a reason for hiding this comment

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

I confirmed it works well in curves.
image

@brkay54 brkay54 merged commit d61f622 into autowarefoundation:main Sep 2, 2022
@tier4-autoware-public-bot tier4-autoware-public-bot bot mentioned this pull request Sep 3, 2022
Merged
@mehmetdogru mehmetdogru mentioned this pull request Sep 7, 2022
Merged
7 tasks
boyali referenced this pull request in boyali/autoware.universe Sep 28, 2022
@brkay54 @pre-commit-ci @boyali
refactor(motion_velocity_smoother): change curvature calculation (tie…
c496760 
…r4#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali referenced this pull request in boyali/autoware.universe Oct 3, 2022
@brkay54 @pre-commit-ci @boyali
refactor(motion_velocity_smoother): change curvature calculation (tie…
045ea98 
…r4#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali referenced this pull request in boyali/autoware.universe Oct 3, 2022
@brkay54 @pre-commit-ci @boyali
refactor(motion_velocity_smoother): change curvature calculation (tie…
dee25d2 
…r4#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
yukke42 pushed a commit to tzhong518/autoware.universe that referenced this pull request Oct 14, 2022
@brkay54 @pre-commit-ci
refactor(motion_velocity_smoother): change curvature calculation (aut…
c433ebf 
…owarefoundation#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali referenced this pull request in boyali/autoware.universe Oct 19, 2022
@brkay54 @pre-commit-ci @boyali
refactor(motion_velocity_smoother): change curvature calculation (tie…
945acde 
…r4#1638)

* refactor(motion_velocity_smoother): change curvature calculation

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* add error handler for too close points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

* take copy for first and last points

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

* ci(pre-commit): autofix

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
@brkay54 brkay54 deleted the update-curvature-calculation branch April 27, 2023 22:50
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@TakaHoribe TakaHoribe TakaHoribe approved these changes

@mehmetdogru mehmetdogru Awaiting requested review from mehmetdogru

@tkimura4 tkimura4 Awaiting requested review from tkimura4

Assignees

@brkay54 brkay54

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@brkay54 @TakaHoribe