feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc #1405

soblin · 2022-07-25T02:00:47Z

feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc

Signed-off-by: Mamoru Sobue mamoru.sobue@tier4.jp

Description

Previously intersection module was calculating passing time to each point on the path from ego position based on the uniform acceleration motion as follows:

v0, v0 + at, v0+2at,,..

where each V0 ,,,, Vn is up to Vmax. This is not realistic given the curve in intersection part. In this PR motion_velocity_smoother is applied to this motion and then ttc is calculated using the smoothed motion.

Tests performed

Tested in Psim at odaiba map.

td_array-2022-07-28_15.51.05.mp4

The gif file shows the passing-time & distance of each point on the path from ego position, for previous(blue) / smoothed (red, this PR) reference motions.

Pre-review checklist for the PR author

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

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

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.
The PR is ready for merge.

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

codecov · 2022-07-25T02:35:59Z

Codecov Report

Merging #1405 (98aea5c) into main (5571338) will decrease coverage by 0.03%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #1405      +/-   ##
==========================================
- Coverage   10.27%   10.24%   -0.04%     
==========================================
  Files        1118     1118              
  Lines       77581    77821     +240     
  Branches    17854    18025     +171     
==========================================
  Hits         7974     7974              
- Misses      62122    62338     +216     
- Partials     7485     7509      +24

Flag	Coverage Δ		*Carryforward flag
differential	`4.75% <0.00%> (?)`
total	`10.26% <0.00%> (ø)`		Carriedforward from 5571338

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...vior_velocity_planner/include/utilization/util.hpp	`0.00% <0.00%> (ø)`
...c/scene_module/intersection/scene_intersection.cpp	`0.00% <0.00%> (ø)`

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 5571338...98aea5c. Read the comment docs.

yukkysaito · 2022-07-28T08:33:53Z

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -465,6 +470,24 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
    return {{0.0, 0.0}};  // has already passed the intersection.
  }

+  PathWithLaneId smoothed_reference_path;
+  if (!smoothPath(reference_path, smoothed_reference_path, planner_data_))


Is it better to output some message?

Yes, I reflected your comment.

yukkysaito

LGTM.
Pls get approval by @kyoichi-sugahara

…eference trajectory Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…ailed Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…ry for ttc (tier4#1405) * feat(behavior_velocity_smoother): simply apply smoother to existing reference trajectory Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * avoid zero division Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * initialize smoothed_path with reference_path and WARN if smoothPath failed Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * added another version for calcDist2d Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…ry for ttc (autowarefoundation#1405) * feat(behavior_velocity_smoother): simply apply smoother to existing reference trajectory Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * avoid zero division Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * initialize smoothed_path with reference_path and WARN if smoothPath failed Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * added another version for calcDist2d Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…ry for ttc (tier4#1405) * feat(behavior_velocity_smoother): simply apply smoother to existing reference trajectory Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * avoid zero division Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * initialize smoothed_path with reference_path and WARN if smoothPath failed Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * added another version for calcDist2d Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…utowarefoundation#1405) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (autowarefoundation#7673)" This reverts commit 69258bd. * dry steering * add comments * add minor fix and modify unit test for dry steering --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

soblin force-pushed the feat/analytical_smoothr_for_intersection_ttc branch from b30bba6 to 4e355fe Compare July 25, 2022 02:11

soblin force-pushed the feat/analytical_smoothr_for_intersection_ttc branch from 4e355fe to 526607e Compare July 25, 2022 03:12

soblin marked this pull request as ready for review July 28, 2022 08:17

soblin changed the title ~~feat(behavior_velocity_smoother): simply apply smoother to existing reference trajectory~~ feat(behavior_velocity_planner): simply apply smoother to existing reference trajectory Jul 28, 2022

soblin requested review from yukkysaito, 1222-takeshi and kyoichi-sugahara July 28, 2022 08:19

soblin changed the title ~~feat(behavior_velocity_planner): simply apply smoother to existing reference trajectory~~ feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc Jul 28, 2022

yukkysaito reviewed Jul 28, 2022

View reviewed changes

soblin force-pushed the feat/analytical_smoothr_for_intersection_ttc branch 2 times, most recently from 2eda5fb to 1ec374f Compare July 28, 2022 09:24

yukkysaito approved these changes Jul 28, 2022

View reviewed changes

soblin force-pushed the feat/analytical_smoothr_for_intersection_ttc branch 2 times, most recently from cf11b5d to 568c85c Compare July 29, 2022 02:25

soblin added 4 commits July 29, 2022 11:28

feat(behavior_velocity_smoother): simply apply smoother to existing r…

e2a3346

…eference trajectory Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

avoid zero division

ccd6223

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

initialize smoothed_path with reference_path and WARN if smoothPath f…

cd361dd

…ailed Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

added another version for calcDist2d

98aea5c

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

soblin force-pushed the feat/analytical_smoothr_for_intersection_ttc branch from 568c85c to 98aea5c Compare July 29, 2022 02:30

soblin enabled auto-merge (squash) July 29, 2022 02:31

soblin merged commit befc35d into autowarefoundation:main Jul 29, 2022

soblin deleted the feat/analytical_smoothr_for_intersection_ttc branch July 29, 2022 03:31

tier4-autoware-public-bot bot mentioned this pull request Jul 30, 2022

chore: sync upstream tier4/autoware.universe#88

Merged

soblin mentioned this pull request Aug 29, 2022

fix(intersection): use constant velocity for smoother reference #1721

Merged

7 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc #1405

feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc #1405

soblin commented Jul 25, 2022 •

edited

Loading

codecov bot commented Jul 25, 2022 •

edited

Loading

yukkysaito Jul 28, 2022

soblin Jul 28, 2022

yukkysaito left a comment

feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc #1405

feat(behavior_velocity_planner): apply smoother to reference trajectory for ttc #1405

Conversation

soblin commented Jul 25, 2022 • edited Loading

Description

Tests performed

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Jul 25, 2022 • edited Loading

Codecov Report

yukkysaito Jul 28, 2022

Choose a reason for hiding this comment

soblin Jul 28, 2022

Choose a reason for hiding this comment

yukkysaito left a comment

Choose a reason for hiding this comment

soblin commented Jul 25, 2022 •

edited

Loading

codecov bot commented Jul 25, 2022 •

edited

Loading