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

Nav2 route bfs #3536

Draft
wants to merge 37 commits into
base: nav2_route_server
Choose a base branch
from

Conversation

jwallace42
Copy link
Contributor


Basic Info

Info Please fill out this column
Ticket(s) this addresses (add tickets here #1)
Primary OS tested on (Ubuntu, MacOS, Windows)
Robotic platform tested on (Steve's Robot, gazebo simulation of Tally, hardware turtlebot)

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:

  • Check that any new parameters added are updated in navigation.ros.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

@mergify
Copy link
Contributor

mergify bot commented Apr 21, 2023

@jwallace42, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @nav2_route_server, but it must be in main
to have these changes reflected into new distributions.

@jwallace42 jwallace42 marked this pull request as draft April 24, 2023 17:26
jwallace42 and others added 11 commits May 24, 2023 09:42
* 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]>
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/include/nav2_route/breadth_first_search.hpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

Close!

nav2_route/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_route/include/nav2_route/breadth_first_search.hpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

You can tell when its close to being done with something had looks easy and obvious. Nice work!

nav2_route/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_route/include/nav2_route/breadth_first_search.hpp Outdated Show resolved Hide resolved
nav2_route/include/nav2_route/breadth_first_search.hpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

You appear to have some IO detail problems where things aren't being set or outputs actually used and given to the requester to complete the task usefully

nav2_route/package.xml Outdated Show resolved Hide resolved
nav2_route/include/nav2_route/goal_intent_extractor.hpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
nav2_route/src/route_server.cpp Outdated Show resolved Hide resolved
nav2_route/src/route_server.cpp Show resolved Hide resolved
nav2_route/src/graph_loader.cpp Outdated Show resolved Hide resolved
nav2_route/src/node_spatial_tree.cpp Outdated Show resolved Hide resolved
nav2_route/src/node_spatial_tree.cpp Show resolved Hide resolved
nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
bfs_->clearGraph();
bfs_->setStart(s_mx, s_my);
bfs_->setGoals(goals);
if (bfs_->isNodeVisible()) {
Copy link
Member

Choose a reason for hiding this comment

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

isFirstGoalVisible

nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
nav2_route/src/breadth_first_search.cpp Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

You have a few functions where you do costmap_sub->getCostmap()->Fn() multiple times. Store a ref to costmap_sub->getCostmap() so its more readable that you're just calling costmap->Fn()

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

And still some of the open things that are more major :-) Esp. wrt search costs

}

// Check for out of bounds
if (neighbor_index >= max_index_) {
Copy link
Member

Choose a reason for hiding this comment

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

Check this on L118 with < 0

Copy link
Contributor Author

Choose a reason for hiding this comment

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

It is a unsigned int so it should never be negative or something else can seriously wrong.

Copy link
Member

Choose a reason for hiding this comment

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

    int index = parent_index + neighbors_grid_offset;
    if (index < 0) {
      continue;
    }

Check the >= there.

nav2_route/src/goal_intent_extractor.cpp Outdated Show resolved Hide resolved
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(
Copy link
Member

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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.

Copy link
Member

@SteveMacenski SteveMacenski Sep 18, 2023

Choose a reason for hiding this comment

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

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

@jwallace42
Copy link
Contributor Author

jwallace42 commented Oct 18, 2023

Things left.

  1. test search on local costmap.
  2. Add fallback parameter if goal cannot be found.
  3. Benchmark.

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.

2 participants