fix(behavior_path_planner): change the order of resampling and smooth goal connection #1381
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Signed-off-by: Takayuki Murooka <[email protected]>
|
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Description
beta0.3.16とbeta/v0.3.18.1の経路計画を比較した結果、初期値とゴール位置の距離が近い場合に、
初期位置周辺の経路計画の差分が大きい問題を対策するPRとなる。
原因
ゴールに対してスムーズな経路計画を実施している。
このスムーズな経路計画を実現するために、ゴール周辺の経路計画の点を削除して、一定距離からゴールに向かって、
スムーズな経路計画を実施する。
ゴールに対してスムーズな経路計画を実施するための対象となる点が、
beta0.3.16とbeta/v0.3.18.1で異なっている事により、経路計画の差分が大きくなる問題が発生している。
ゴールに対してスムーズな経路計画を実施するための対象となる点が異なる理由
ゴールに対してスムーズな経路計画の実施を行う入力時の経路が、beta0.3.16とbeta/v0.3.18で異なるためである。
具体的には、beta0.3.17で、経路歪み対策としてリサンプリング機能を追加実装したが、
リサンプリング機能により、経路計画の点が増えており、ゴールに対してスムーズな経路計画を実施時に、
削除対象となる点が変更となっている。
以下、beta0.3.16とbeta/v0.3.18のゴールに対するスムーズな経路計画のロジックをまとめる。
beta0.3.16の経路計画
ロジックのフロー
beta/v0.3.18の挙動
ロジックのイメージ
ロジックのフロー
beta0.3.16とv4.3.0のゴールに対してスムーズな経路計画の比較
以下のように、初期位置付近の経路が直線と曲線で変化しており、
この変化がそのまま経路差分となる。
対策
そのため、「スムーズなゴールに対する経路計画」を実施後に、リサンプリングを行うようにする。
これにより、beta0.3.16とbeta/v0.3.18で同じ形状の経路計画が実施されるようになる。
ロジックのフロー
影響
リサンプリグの実施順序が変更になっただけで、負荷的な影響はない。
Related links
https://tier4.atlassian.net/browse/AEAP-1333
https://tier4.atlassian.net/browse/AEAP-661
Tests performed
機能テスト
特定経路のstop pointから近い位置に、初期値を設定し、goal poseまでのを、beta0.3.16とbeta/v0.3.18で比較し差分が、
2.5cm以内である事を確認。
＊詳細は、チケット参照。
リグレッションテスト
経路計画に対するリサンプリング機能を追加する要因となった、
重複経路における経路歪みが発生しない事を確認済み
実車テスト
リリース評価時に実施する
Effects on system behavior
Not applicable.
Interface changes
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.
After all checkboxes are checked, anyone who has write access can merge the PR.