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.

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

fix(behavior_path_planner): improve isPathInLanelet function for lane change #2693

Merged

Conversation

zulfaqar-azmi-t4
Copy link
Contributor

@zulfaqar-azmi-t4 zulfaqar-azmi-t4 commented Jan 19, 2023

Description

isPathInLanelet function consume the most resources when constructing lane change path. The reason is due to the necessity to check each point on the path against the reference and target lane. Because the computation is based on boost distance, it also takes quite some time. This PR attempt to improve this by considering covered_by function.

Related links

Tests performed

Notes for reviewers

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.

  • 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.

@github-actions github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Jan 19, 2023
@zulfaqar-azmi-t4 zulfaqar-azmi-t4 force-pushed the fix-lc-topic-drop-3 branch 2 times, most recently from 6bd5f6b to a21be1f Compare January 19, 2023 04:44
… change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
@codecov
Copy link

codecov bot commented Jan 19, 2023

Codecov Report

Base: 11.38% // Head: 11.35% // Decreases project coverage by -0.03% ⚠️

Coverage data is based on head (7078956) compared to base (9e3e3a0).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files
@@            Coverage Diff             @@
##             main    #2693      +/-   ##
==========================================
- Coverage   11.38%   11.35%   -0.03%     
==========================================
  Files        1277     1277              
  Lines       89267    89469     +202     
  Branches    23641    23783     +142     
==========================================
  Hits        10160    10160              
- Misses      68336    68529     +193     
- Partials    10771    10780       +9     
Flag Coverage Δ *Carryforward flag
differential 4.93% <0.00%> (?)
total 11.38% <0.00%> (ø) Carriedforward from 9e3e3a0

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

Impacted Files Coverage Δ
...path_planner/src/scene_module/lane_change/util.cpp 0.00% <0.00%> (ø)

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

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

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.

works well. (But still, computational cost needs to be improved. Now it is 5Hz in my desktop PC)

Before
image

After
image

@zulfaqar-azmi-t4 zulfaqar-azmi-t4 merged commit 87641bf into autowarefoundation:main Jan 19, 2023
@zulfaqar-azmi-t4 zulfaqar-azmi-t4 deleted the fix-lc-topic-drop-3 branch January 19, 2023 07:59
tkimura4 pushed a commit to tier4/autoware.universe that referenced this pull request Jan 24, 2023
… change (autowarefoundation#2693)

* fix(behavior_path_planner): improve isPathInLanelet function for lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* simplify the functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
tkimura4 added a commit to tier4/autoware.universe that referenced this pull request Jan 25, 2023
* fix(behavior_path_planner): lane change check default parameters (autowarefoundation#2575)

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* feat(lane_change): update path generation logic to consider lateral jerk and lateral acceleration (autowarefoundation#2428)

* [lane_change] update path generation to handle lateral acceleration limit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code & fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update docs

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move path_shifter implementation to cpp

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* update doc link

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add common min distance computation

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* rearrange config and rework min distance

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* revert some changes

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove warning

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix spell check

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(behavior_path_planner): abort lane change function (autowarefoundation#2359)

* feat(behavior_path_planner): abort lane change function

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* change Revert -> Cancel

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Remove some unwanted functions and and STOP state

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* update steering factor (accidentally removed)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* include is_abort_condition_satisfied_ flag

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* use only check ego in current lane

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Revert "use only check ego in current lane"

This reverts commit 4f97408.

* ci(pre-commit): autofix

* use only check ego in current lane

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* improve isAbortConditionSatisfied by using ego polygon check

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* add lateral jerk and path doesn't keep on updating anymore

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* parameterized all abort related values

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* rename abort_end -> abort_return

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix some parameter issue

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* check if lane change distance is enough after abort

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* improve the code flow of isAbortConditionSatisfied

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Place warning message in corresponding states.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix clock and rebase

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove accel and jerk parameters

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove unnecessary parameters

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix param file in config

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove isStopState and refactoring

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Fixed CANCEL when ego is out of lane

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix path reset during abort

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix abort path exceed goal

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix logger to debug

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* feat(behavior path planner): lane change cancel/abort docs update (autowarefoundation#2599)

* feat(behavior path planner): lane change cancel/abort docs update

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update parameters and it's config (yaml) file

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix(lane_change): use current lane for num to preferred lane input (autowarefoundation#2615)

* fix(lane_change): use current lane for num to preferred lane input

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix lane change distance from deadend

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Make separate function to compute resampling interval

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Change default config

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Make phase info data structure

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Added error for finish judge buffer

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Fix rebase

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* ci(pre-commit): autofix

* warn user of the modified values

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix(behavior_path_planner): lane change turn signal during approval (autowarefoundation#2645)

* fix(behavior_path_planner): lane change turn signal during approval

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* some refactoring

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix(behavior_path_planner): improve isPathInLanelet function for lane change (autowarefoundation#2693)

* fix(behavior_path_planner): improve isPathInLanelet function for lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* simplify the functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* feat(rtc_auto_mode_manager)!: add auto mode status array (autowarefoundation#2517)

* feat(rtc_aut_mode_manager)!: add auto mode status array

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* chore:  planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* feat(behavior_path_planner): external request lane change (autowarefoundation#2442)

* feature(behavior_path_planner): add external request lane change module

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feature(behavior_path_planner): fix for RTC

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feature(behavior_path_planner): fix decision logic

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): fix behavior_path_planner_tree.xml

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): fix for rebase

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): output multiple candidate paths

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): get path candidate in behavior tree manager

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): fix for multiple candidate path

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): separate external request lane change module

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feature(behavior_path_planner): add create publisher method

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feature(behavior_path_planner): move publishers to node

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feature(behavior_path_planner): remove unnecessary publisher

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): move reset path candidate function to behavior tree manager

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): add external request lane change path candidate publisher

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

feat(behavior_path_planner): apply abort lane change

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): remove unnecessary change

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(behavior_path_planner): fix getLaneChangePaths()

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(behavior_path_planner): disable external request lane change in default tree

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* Update rtc_auto_mode_manager.param.yaml

* fix(route_handler): remove redundant code

* fix(behavior_path_planner): fix for turn signal

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix(motion_utils/path_shifter): modify the sampling method (autowarefoundation#2658)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix(behavior_path_planner): lane change interpolated obj orientation (autowarefoundation#2727)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix(behavior_path_planner): reduce obj indices call in lane change (autowarefoundation#2726)

* fix(behavior_path_planner): reduce obj indices call in lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove unused functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* add continue for the intersect in current

* replace target and current lanes

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* change isLaneChangePathSafe arguments

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* feat(behavior_path_planner): enable lane change in intersection (autowarefoundation#2720)

fix(behavior_path_planner): enable lane change in intersection

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): make lane change safety check adaptive (autowarefoundation#2704)

* fix(behavior_path_planner): make lane change safety check adaptive

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Temporarily hard code use all predicted path

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Revert "Temporarily hard code use all predicted path"

This reverts commit 8f92e45.

* fix external lane change request

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* use prediction resolution as rounding multiplier

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* fix(lane_change): chattering issue when performing check (autowarefoundation#2741)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
maxime-clem pushed a commit to maxime-clem/autoware.universe that referenced this pull request Jan 30, 2023
… change (autowarefoundation#2693)

* fix(behavior_path_planner): improve isPathInLanelet function for lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* simplify the functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
lexavtanke pushed a commit to lexavtanke/autoware.universe that referenced this pull request Jan 31, 2023
… change (autowarefoundation#2693)

* fix(behavior_path_planner): improve isPathInLanelet function for lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* simplify the functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants