planner_3d: fix cost calculation bug on in-place turning #725
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
This comment has been minimized.
This comment has been minimized.
Codecov Report
@@ Coverage Diff @@
## master #725 +/- ##
==========================================
+ Coverage 88.67% 88.87% +0.20%
==========================================
Files 62 62
Lines 4582 4584 +2
==========================================
+ Hits 4063 4074 +11
+ Misses 519 510 -9
|
at-wat
reviewed
Oct 30, 2023
at-wat
changed the title
This comment has been minimized.
This comment has been minimized.
|
I added small tests in test_planner_3d_cost.cpp. |
This comment has been minimized.
This comment has been minimized.
[528] PASSED on noeticAll tests passed |
nhatao
changed the title
nhatao
changed the title
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.
This PR fixes a bug in
GridAstarModel3D::cost().When the movement between
curandnextis an in-place turning,cm_[cur]is not evaluated. I think it is a simple mistake.In contrast, when the movement between
curandnextis not an in-place turning,MotionCacheis used to interpolate the movement of the robot. BecauseMotionCache::Page::getMotion()does not return the last pose (I guess this is needed to avoid double counting of costs), thecm_[next]is not evaluated. Therefore, when only thecm_[next]is 100 and the costs of other poses are not 100,GridAstarModel3D::cost()returns a positive value mistakenly.Because of these bugs, the planner_3d could create a path in which the robot collides with an obstacle in the following situation.
Assume that the cost of the grid (10, 5, 0) is 100, and the costs of other grids are not 100. For example, the path (5, 5, 0) -> (10, 5, 0) -> (10, 5, 3) can be generated.
The interpolated poses between (5, 5, 0) to (10, 5, 0) generated by
MotionCachedo not include (10, 5, 0) as it is the last pose, so the cost of the grid (10, 5, 0) is not evaluated.The interpolated poses between (10, 5, 0) to (10, 5, 3) do not include the first pose (10, 5, 0) because of the bug in the cost calculation of in-place turning motion.
As a result, the cost of (10, 5, 0) is never evaluated and planner_3d can generate an untraversable path.