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_cspace: fix error status handling after cost_estim_cache_ was not created #705

Merged
merged 10 commits into from
Jul 7, 2023

Conversation

nhatao
Copy link
Collaborator

@nhatao nhatao commented Jul 5, 2023

In the current master branch, When cnt_stuck_ has a positive value, status.error becomes PATH_NOT_FOUND. The value of cnt_stuck_ is incremented when the start or the goal cell is occupied, and it is retained even after the occupied cell is cleared and path planning is succeeded. Therefore, the status.error never returns to GOING_WELL in such cases. In addition, when the start cell is occupied, status.error should become IN_ROCK.
This bug is introduced by db25329.

In this PR, cost_estim_cache_created_ is used to check if the start or goal cell is occupied instead of cnt_stuck_.
In addition, is_start_occupied_ flag is added to distinguish IN_ROCK status.
Some inappropriate names of functions and variables are renamed.

@nhatao nhatao changed the title planner_cspace: reset cnt_stuck_ when both of goal and start cells ar… planner_cspace: reset cnt_stuck_ when both of goal and start cells are cleared Jul 5, 2023
@nhatao nhatao changed the title planner_cspace: reset cnt_stuck_ when both of goal and start cells are cleared planner_cspace: reset cnt_stuck_ when goal and start cells are cleared Jul 5, 2023
@at-wat

This comment has been minimized.

@codecov-commenter
Copy link

codecov-commenter commented Jul 5, 2023

Codecov Report

Merging #705 (c189a14) into master (4c9f1a3) will increase coverage by 0.18%.
The diff coverage is 88.57%.

@@            Coverage Diff             @@
##           master     #705      +/-   ##
==========================================
+ Coverage   88.13%   88.31%   +0.18%     
==========================================
  Files          60       60              
  Lines        4323     4330       +7     
==========================================
+ Hits         3810     3824      +14     
+ Misses        513      506       -7     
Impacted Files Coverage Δ
planner_cspace/src/planner_3d.cpp 85.13% <88.23%> (+1.17%) ⬆️
planner_cspace/src/patrol.cpp 89.28% <100.00%> (ø)

... and 1 file with indirect coverage changes

@nhatao nhatao requested a review from at-wat July 5, 2023 09:57
@nhatao nhatao changed the title planner_cspace: reset cnt_stuck_ when goal and start cells are cleared planner_cspace: fix error status handling Jul 5, 2023
@at-wat

This comment has been minimized.

@nhatao nhatao removed the request for review from at-wat July 5, 2023 14:27
@nhatao nhatao marked this pull request as draft July 5, 2023 14:27
@nhatao
Copy link
Collaborator Author

nhatao commented Jul 5, 2023

There is still a problem. Changed to WIP

@at-wat

This comment has been minimized.

@at-wat

This comment has been minimized.

@nhatao nhatao changed the title planner_cspace: fix error status handling planner_cspace: fix error status handling after cost_estim_cache_ was not created Jul 6, 2023
@at-wat

This comment has been minimized.

@at-wat

This comment has been minimized.

@at-wat

This comment has been minimized.

@at-wat

This comment has been minimized.

@nhatao nhatao marked this pull request as ready for review July 6, 2023 12:07
@nhatao nhatao requested a review from at-wat July 6, 2023 12:07
@at-wat
Copy link
Owner

at-wat commented Jul 6, 2023

[459] PASSED on noetic

All tests passed
build/test_results/costmap_cspace/gtest-test_costmap_3d.xml: 22 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: 14 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_path_interpolator.xml: 4 tests
build/test_results/planner_cspace/gtest-test_planner_3d_cost.xml: 2 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_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_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: 8 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: 315 tests, 0 errors, 0 failures, 0 skipped

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 636aa48 into master Jul 7, 2023
@at-wat at-wat deleted the reset-cnt-stuck branch July 7, 2023 01:07
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.

3 participants