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

planner_3d: remove duplicated interpolation method #733

Merged
merged 6 commits into from
Mar 22, 2024

Conversation

nhatao
Copy link
Collaborator

@nhatao nhatao commented Mar 14, 2024

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 is PathInterpolator, which is used to make a smooth path after path planning. In the master branch, the resolution of the path interpolated by MotionCache is rougher than the path interpolated by PathInterpolator. Therefore, occupied grids that were not determined to be passed through during the path planning procedure may be included in the path interpolated by the PathInterpolator. 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

@@ -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);
Copy link
Collaborator Author

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.

@at-wat

This comment has been minimized.

@nhatao nhatao marked this pull request as draft March 14, 2024 13:06
@at-wat

This comment has been minimized.

@codecov-commenter
Copy link

codecov-commenter commented Mar 19, 2024

Codecov Report

Attention: Patch coverage is 91.22807% with 5 lines in your changes are missing coverage. Please review.

Project coverage is 88.79%. Comparing base (65632b0) to head (77e3ddf).

Files Patch % Lines
planner_cspace/src/motion_cache.cpp 92.50% 3 Missing ⚠️
planner_cspace/src/planner_3d.cpp 75.00% 2 Missing ⚠️
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.
📢 Have feedback on the report? Share it here.

@at-wat

This comment has been minimized.

@nhatao nhatao marked this pull request as ready for review March 19, 2024 07:09
@nhatao nhatao requested a review from at-wat March 19, 2024 07:09
@nhatao nhatao changed the title Remove duplicated interpolation method planner_3d: remove duplicated interpolation method Mar 19, 2024
@at-wat

This comment has been minimized.

@@ -71,6 +73,7 @@ GridAstarModel3D::GridAstarModel3D(
, cm_rough_(cm_rough)
, cc_(cc)
, range_(range)

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

Comment on lines 140 to 153
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]);
Copy link
Owner

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?

Copy link
Collaborator Author

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

@at-wat
Copy link
Owner

at-wat commented Mar 22, 2024

[554] PASSED on noetic

All tests passed
build/test_results/costmap_cspace/gtest-test_costmap_3d.xml: 24 tests
build/test_results/costmap_cspace/gtest-test_pointcloud_accumulator.xml: 4 tests
build/test_results/costmap_cspace/roslint-costmap_cspace.xml: 1 tests
build/test_results/joystick_interrupt/roslint-joystick_interrupt.xml: 1 tests
build/test_results/joystick_interrupt/rostest-test_test_joystick_interrupt_rostest.xml: 1 tests
build/test_results/joystick_interrupt/rosunit-test_joystick_interrupt.xml: 12 tests
build/test_results/map_organizer/roslint-map_organizer.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_map_organizer_rostest.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_pointcloud_to_maps_rostest.xml: 1 tests
build/test_results/map_organizer/rosunit-test_map_organizer.xml: 8 tests
build/test_results/map_organizer/rosunit-test_pointcloud_to_maps.xml: 2 tests
build/test_results/neonavigation_common/roslint-neonavigation_common.xml: 1 tests
build/test_results/neonavigation_common/rostest-test_test_compat_rostest.xml: 1 tests
build/test_results/neonavigation_common/rosunit-test_compat.xml: 6 tests
build/test_results/neonavigation_metrics_msgs/gtest-test_helper.xml: 4 tests
build/test_results/neonavigation_metrics_msgs/roslint-neonavigation_metrics_msgs.xml: 1 tests
build/test_results/obj_to_pointcloud/roslint-obj_to_pointcloud.xml: 1 tests
build/test_results/obj_to_pointcloud/rostest-test_test_obj_to_pointcloud_rostest.xml: 1 tests
build/test_results/obj_to_pointcloud/rosunit-test_obj_to_pointcloud.xml: 2 tests
build/test_results/planner_cspace/gtest-test_blockmem_gridmap.xml: 10 tests
build/test_results/planner_cspace/gtest-test_costmap_bbf.xml: 4 tests
build/test_results/planner_cspace/gtest-test_cyclic_vec.xml: 14 tests
build/test_results/planner_cspace/gtest-test_distance_map.xml: 18 tests
build/test_results/planner_cspace/gtest-test_distance_map_fast_update.xml: 4 tests
build/test_results/planner_cspace/gtest-test_grid_astar.xml: 12 tests
build/test_results/planner_cspace/gtest-test_grid_metric_converter.xml: 4 tests
build/test_results/planner_cspace/gtest-test_motion_cache.xml: 2 tests
build/test_results/planner_cspace/gtest-test_motion_primitive_builder.xml: 2 tests
build/test_results/planner_cspace/gtest-test_planner_3d_cost.xml: 2 tests
build/test_results/planner_cspace/gtest-test_start_pose_predictor.xml: 6 tests
build/test_results/planner_cspace/roslint-planner_cspace.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true__fast_map_update_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__with_tolerance_true.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_abort_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_costmap_watchdog_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_debug_outputs_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_dynamic_parameter_change_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_boundary_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_compat_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_2dof_serial_joints_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_3d_map_size_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_preempt_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_tolerant_action_rostest.xml: 1 tests
build/test_results/planner_cspace/rosunit-test_abort.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_costmap_watchdog.xml: 4 tests
build/test_results/planner_cspace/rosunit-test_debug_outputs.xml: 8 tests
build/test_results/planner_cspace/rosunit-test_dynamic_parameter_change.xml: 6 tests
build/test_results/planner_cspace/rosunit-test_navigate.xml: 12 tests
build/test_results/planner_cspace/rosunit-test_navigate_boundary.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_planner_2dof_serial_joints.xml: 4 tests
build/test_results/planner_cspace/rosunit-test_planner_3d_map_size.xml: 12 tests
build/test_results/planner_cspace/rosunit-test_preempt.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_tolerant_action.xml: 2 tests
build/test_results/safety_limiter/roslint-safety_limiter.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter2_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_compat_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_rostest.xml: 1 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter.xml: 22 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter2.xml: 2 tests
build/test_results/track_odometry/gtest-test_tf_projection.xml: 2 tests
build/test_results/track_odometry/roslint-track_odometry.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_tf_projection_rostest.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_track_odometry_rostest.xml: 1 tests
build/test_results/track_odometry/rosunit-test_tf_projection_node.xml: 8 tests
build/test_results/track_odometry/rosunit-test_track_odometry.xml: 10 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_filter.xml: 6 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_path2d.xml: 18 tests
build/test_results/trajectory_tracker/roslint-trajectory_tracker.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_recorder_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_overshoot_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_with_odom_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-trajectory_tracker_rostest__odom_delay_0.xml: 1 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_recorder.xml: 2 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker.xml: 16 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_overshoot.xml: 12 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_with_odom.xml: 4 tests
build/test_results/trajectory_tracker_msgs/gtest-test_path_with_velocity_conversion.xml: 4 tests
build/test_results/trajectory_tracker_msgs/roslint-trajectory_tracker_msgs.xml: 1 tests
Summary: 340 tests, 0 errors, 0 failures, 0 skipped

@nhatao nhatao requested a review from at-wat March 22, 2024 01:47
Copy link
Owner

@at-wat at-wat left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@at-wat at-wat merged commit 59cab2d into master Mar 22, 2024
3 checks passed
@at-wat at-wat deleted the fix-repeatedly-plan-fail branch March 22, 2024 03:52
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants