Nav2 route bfs #3536
Nav2 route bfs #3536
Conversation
|
@jwallace42, all pull requests must be targeted towards the |
* Split overlay setup into multiple steps by skipping slower to build leaf packages during preparation, then store cache and repeat setup without skipping packages * Skip restore steps after already preping overlay to avoid needlessly downloading the same overlay cache * Revert resource_class to default medium as the build resource usage seldom maxes out 4 cores nor uses more than 2GB RAM * Fix circleci config syntax by setting skip default as empty string to keep it an optional parameter * Fix circleci config syntax missing angle brackets * ignore warning * Revert "Revert resource_class to default medium" This reverts commit 44375a1. * Fix nested defaults to avoid dropping of cache after storing during test jobs by ensuring restore_overlay_workspace still sets restore: true --------- Co-authored-by: ruffsl <[email protected]>
* Split overlay setup into multiple steps by skipping slower to build leaf packages during preparation, then store cache and repeat setup without skipping packages * Skip restore steps after already preping overlay to avoid needlessly downloading the same overlay cache * Revert resource_class to default medium as the build resource usage seldom maxes out 4 cores nor uses more than 2GB RAM * Fix circleci config syntax by setting skip default as empty string to keep it an optional parameter * Fix circleci config syntax missing angle brackets * ignore warning * Revert "Revert resource_class to default medium" This reverts commit 44375a1. * Fix nested defaults to avoid dropping of cache after storing during test jobs by ensuring restore_overlay_workspace still sets restore: true --------- Co-authored-by: ruffsl <[email protected]>
| bfs_->clearGraph(); | ||
| bfs_->setStart(s_mx, s_my); | ||
| bfs_->setGoals(goals); | ||
| if (bfs_->isNodeVisible()) { |
|
You have a few functions where you do |
| } | ||
|
|
||
| // Check for out of bounds | ||
| if (neighbor_index >= max_index_) { |
There was a problem hiding this comment.
Check this on L118 with < 0
There was a problem hiding this comment.
It is a unsigned int so it should never be negative or something else can seriously wrong.
There was a problem hiding this comment.
int index = parent_index + neighbors_grid_offset;
if (index < 0) {
continue;
}
Check the >= there.
| float goal_x = goal.pose.position.x; | ||
| float goal_y = goal.pose.position.y; | ||
| if (!costmap_sub_->getCostmap()->worldToMap(goal_x, goal_y, g_mx, g_my)) { | ||
| RCLCPP_WARN_STREAM( |
There was a problem hiding this comment.
I'd ask that you sanity check that this all finds the right spots and whatnot when in the odom frame in case there's some data somewhere else not being properly transformed
There was a problem hiding this comment.
I think we should think about the cases were we plan to use the local costmap.
The current implementation doesn't quite make sense. I good example when we are trying to do goal association. We will likely be outside of the local costmap bounds.
I think it only makes sense if, use start is false and we only associate the start.
There was a problem hiding this comment.
Its less about the specifics of a rolling local costmap as much as it as supporting a rolling costmap in general. The GPS threads are making me think about the need to make all the servers accept rolling global costmaps (which most will actually already) due to navigating very large spaces that you either don't have a map or its unrealistic to have it all loaded at once.
See Slack comment on this
|
Things left.
|
Basic Info
Description of contribution in a few bullet points
This pr adds a breadth first search algorithm to the nav2_route. It returns the closest goal in a list of goals.
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: