fix(path_generator): bug fix for unstable behavior when the ego vehicle is close to goal #361
Conversation
This file contains hidden or 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: Junya Sasaki <junya.sasaki@tier4.jp>
* This bug occurred when the distance between the end of
the path and the goal was less than 1.0 [m].
In such cases, the orientation to the pre-goal was unexpectedly
inverted by π [rad].
* This commit resolves the issue by applying the correct offset dynamically,
instead of relying on a hard-coded offset of 1.0 [m].
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
* When the goal is very close (<= 1 [m]),
- Wrong pose pre goal is inserted: this causes inverted orientation
- Displaced velocity is inserted
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
|
Thank you for contributing to the Autoware project! 🚧 If your pull request is in progress, switch it to draft mode. Please ensure:
|
kosuke55
reviewed
Apr 9, 2025
Comment on lines
431
to
435
| // Use the original path | ||
| finalized_path_with_lane_id = std::move(preprocessed_path); | ||
|
|
||
| // But set the velocity of the goal point to 0 for safety | ||
| finalized_path_with_lane_id.points.back().point.longitudinal_velocity_mps = 0.0; |
There was a problem hiding this comment.
Does this process result in a sudden path change and sudden steering if is_goal_over turns true?
There was a problem hiding this comment.
Thank you for a good question. Let me check.
There was a problem hiding this comment.
Yes, you are right! Now I'm fixing the non-intended behavior 👍
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
sasakisasaki
changed the title
sasakisasaki
changed the title
|
Somehow, after merging main into this PR, the goal connection is not performed. I'm investigating the reason. |
|
Perhaps I find the reason though not fully understand yet. Performing the goal connection after the crop by trajectory makes it succeeded. Now seeing the behavior on the Psim, but today I'll go back home soon (maybe the updated report will be tomorrow). |
* Remove unnecessary logic after the goal
- This might cause unexpected behavior
* Do goal connection after the crop by trajectory
- It seems before the crop, the tail of trajectory
becomes far from the goal point and the goal connection
function `modify_path_for_smooth_goal_connection` is not
executed because of the following if condition:
```
distance_between_goal_and_path_end < params.refine_goal_search_radius_range && !is_goal_over
```
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
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
This PR fixes following unstable behavior of ego vehicle when the goal is close. This bug appears especially when using short backward length.
Related links
Discussion in TIER IV internal slack (sorry that only limited person can see)
How was this PR tested?
The bug is frequently appears when the backward length is small (<= 0.5 [m]). I verified that the bug is fixed in the test with Psim. Also to detect the regression due to changes by this PR, I also checked with the TIER IV internal evaluator. Considering these points, our tests strategy is as following two.
Tests with Psim
(9th April 2025) If you are trying to build the Autoware, perhaps you need to use
autoware_lanelet2_extensionwithv0.6.4.The build procedure is very close to that of this installation tutorial. But you need to change the parameter (behavior_path_planner_type) here to switch to use
autoware_path_generator.(preparing the video here)
Tests on TIER IV internal evaluator
Notes for reviewers
Sorry for inconvenience due to the bugs I embedded.
Interface changes
None.
Effects on system behavior
The bugs in the description will be fixed after merging this PR. Hopefully, this helps
autoware_coreusers.