-
Notifications
You must be signed in to change notification settings - Fork 91
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
planner_3d: remove duplicated interpolation method #733
Conversation
@@ -68,7 +68,7 @@ class StartPosePredictorTester : public ::testing::Test | |||
map_info_.origin.orientation.w = 1.0; | |||
map_info_.linear_resolution = 1.0; | |||
map_info_.angular_resolution = 2 * M_PI / map_info_.angle; | |||
interpolator_.reset(map_info_.linear_resolution, 1); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The previous test was wrong:
interpolator_.reset(map_info_.linear_resolution, 1);
should be
interpolator_.reset(map_info_.angular_resolution, 1);
It is needed to change the SwitchBack
test slightly with the correct interpolations.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## master #733 +/- ##
==========================================
- Coverage 88.92% 88.79% -0.14%
==========================================
Files 62 60 -2
Lines 4606 4596 -10
==========================================
- Hits 4096 4081 -15
- Misses 510 515 +5 ☔ View full report in Codecov by Sentry. |
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
@@ -71,6 +73,7 @@ GridAstarModel3D::GridAstarModel3D( | |||
, cm_rough_(cm_rough) | |||
, cc_(cc) | |||
, range_(range) | |||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done
planner_cspace/src/motion_cache.cpp
Outdated
const CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]); | ||
CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is it required to remove const
?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done. posf_raw
is removed
[554] PASSED on noeticAll tests passed
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
LGTM
Sometimes path planning repeatedly succeeds and fails in narrow places with the true parameter
keep_a_part_of_previous_path_
.This is because 2 similar interpolation methods generate inconsistent results. One is
MotionCache
, which is used to enumerate the grids the robot passes through during the path planning procedure. The other isPathInterpolator
, which is used to make a smooth path after path planning. In the master branch, the resolution of the path interpolated byMotionCache
is rougher than the path interpolated byPathInterpolator
. Therefore, occupied grids that were not determined to be passed through during the path planning procedure may be included in the path interpolated by thePathInterpolator
. In the next frame, the StartPosePredictor determines that the previous path collides with obstacles. (The message "The robot might collide with an obstacle during the next planning. An empty path is published." is shown.)In this PR, the PathInterpolator is integrated into
MotionCache
and add the constraint that the resolution of the interpolation for the path smoothing is greater than or equal to the resolution of interpolation for the grid enumeration to avoid generating inconsistent interpolation results. New parameters path_interpolation_resolution and grid_enumeration_resolution are added to change these resolutions.Master branch:
simplescreenrecorder-2024-03-19_15.13.10.mp4
This PR:
simplescreenrecorder-2024-03-19_15.14.24.mp4