Skip to content

Conversation

@leander-dsouza
Copy link
Contributor


Basic Info

Info Please fill out this column
Ticket(s) this addresses N/A
Primary OS tested on Ubuntu
Robotic platform tested on Turtlebot4 Gazebo simulation
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

  • Renamed .geojson graphs so that the map type can be replaced for depot/warehouse.
  • Added dictionary map replacing support for route_example_launch.py.

Description of documentation updates required from your changes

  • N/A

Description of how this change was tested

  • The graph was tested for traversability using the below command:

    ros2 launch nav2_simple_commander route_example_launch.py

    Screenshot from 2025-05-15 04-09-28

    warehouse_route.mp4

Future work that may be required in bullet points

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.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
@leander-dsouza leander-dsouza marked this pull request as ready for review May 15, 2025 03:36
@codecov
Copy link

codecov bot commented May 15, 2025

Codecov Report

All modified and coverable lines are covered by tests ✅

see 8 files with indirect coverage changes

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.
world = os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf')
map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}.yaml')
graph_filepath = os.path.join(
nav2_bringup_dir, 'graphs', f'turtlebot4_graph_{MAP_TYPE}.geojson')
Copy link
Member

Choose a reason for hiding this comment

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

We can rename now to just be depot_graph.geojson and warehouse_graph.geojson instead. We can remove the TB4 prefix if we're having multiple I think. The TB3 we can keep since that's specific to that robot's sandbox

Copy link
Contributor Author

Choose a reason for hiding this comment

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

This should be resolved in the following commit.

"properties": {
"name": "urn:ogc:def:crs:EPSG::3857"
},
"type": "name"
Copy link
Member

Choose a reason for hiding this comment

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

Maybe give is a name :-)

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 have renamed it as warehouse_graph.

@SteveMacenski
Copy link
Member

SteveMacenski commented May 15, 2025

For the graph, I think we can do something a little more structured. I can't quite tell what the rest of the gazebo world looks like, but I wanted to give you a sense of what I was thinking:

image

Note:

  • The warehouse shelves has a rigid, grid-like graph where we have nodes at unique shelves ("picking") and at aisle start/ends for going into the aisle
  • See that the main arteries next to the aisles are bidirectional (both ways) but the inner aisles are single direction (see arrows) so that robots must follow lanes for direction of picks for deconfliction. Robots can go where ever in the main aisles, but once in a shelving aisle, needs to go in a single direction so we don't have robots running into each other.

I can't tell what the bays up top of the map are, if they're more shelves, add a couple of pick spots at each shelf & directional lane above the current shelf lane going to the left (new one going to the right) with the appropriate points to hook into the pick spots in the bay

I assume the 2 things on the left are also shelves that should hook into the graph with a simlar setup.

The "maze" part on the lower left I like what you did but make them more squared up and right angles. Have them continue for a couple of nodes in the end of that corridor. That section can be bidirectional since its narrow.

I'm liking this alot though - I want to get this in for the Kilted release next week! I think you should be proud of this work on the speed, keepout, and graphs, these are really nice features to have more easily available in the default bringup to show the power of these features to users from Day 1. We should make a linkedin/blog post if you have one!

@leander-dsouza
Copy link
Contributor Author

I'm liking this alot though - I want to get this in for the Kilted release next week! I think you should be proud of this work on the speed, keepout, and graphs, these are really nice features to have more easily available in the default bringup to show the power of these features to users from Day 1. We should make a linkedin/blog post if you have one!

Thank you for the kind words, Steve :)
Yes, sure, I will try to get this PR in before next week. I would love to do a joint LinkedIn post on this as well :)

@leander-dsouza
Copy link
Contributor Author

  • The warehouse shelves has a rigid, grid-like graph where we have nodes at unique shelves ("picking") and at aisle start/ends for going into the aisle
  • See that the main arteries next to the aisles are bidirectional (both ways) but the inner aisles are single direction (see arrows) so that robots must follow lanes for direction of picks for deconfliction. Robots can go where ever in the main aisles, but once in a shelving aisle, needs to go in a single direction so we don't have robots running into each other.

I can't tell what the bays up top of the map are, if they're more shelves, add a couple of pick spots at each shelf & directional lane above the current shelf lane going to the left (new one going to the right) with the appropriate points to hook into the pick spots in the bay

I assume the 2 things on the left are also shelves that should hook into the graph with a simlar setup.

The "maze" part on the lower left I like what you did but make them more squared up and right angles. Have them continue for a couple of nodes in the end of that corridor. That section can be bidirectional since its narrow.

I have updated the warehouse as follows. All arrows that are not marked are bidirectional:

Screenshot from 2025-05-16 23-48-21 (Copy)

This is the warehouse world for reference:

image

@leander-dsouza
Copy link
Contributor Author

I am not sure as to why the ament_xmllint is failing. I think the action has some issues, as it passes locally.

@SteveMacenski
Copy link
Member

SteveMacenski commented May 19, 2025

Looks like a networking problem. I retriggered and its solved now.

Last couple of changes:

  • Can the top / bottom aisle on the right also be directional?
  • The 4x waypoints for each shelf isn't in the middle of the shelf, please adjust those positions so that they're in the middle of the shelf
  • The very bottom has no shelves so you can remove the 4x waypoints and just connect the end to end.
  • The very top has no shelves until those 2 on the right, so you can remove the intermediate ones (4x on the left, 1x in the middle between the 2 shelves)
  • The top 2 small shelves look to have 4x shelf units on it, please set 4x waypoints in the center of eahc rather than the 2x we have today (I know they'll be tightly clustered - its actually a good test case)
  • Can we have the 2x large shelves on the left have pick locations in their centers too? If you have that route hug the shelf instead of the wall on the far left, then it can be dual purpose. I think we can skip adding nodes to the shelves creating the 'maze' at the lower left, but we should have nodes for each shelf otherwise. What we can do here is to keep the line down the middle that you have (bidirectional) and then branch off from that to the shelf centers in a straight line orthogonal to it (like a tree). We should have the 'normal' line down the outermost right shelf though as that doesn't have any larger obstructions than the other shelving unit. That should cover the 2x big shelves fully.
  • We should add in a second (or more?) connection in the graph from the units on the right we have covered and the units on the left so its not just a single edge that connects them. For example, it looks like if we extended 2x of the aisle lines on the right they would hit the shelf on the left without any obstructions. Once we have that route line in place, we can add in those nodes to make it a straight line and have additional connection points.

Where does the robot spawn? We may want to add in a node around there if its too too far from the graph.

@leander-dsouza
Copy link
Contributor Author

  • Can the top / bottom aisle on the right also be directional?
  • The 4x waypoints for each shelf isn't in the middle of the shelf, please adjust those positions so that they're in the middle of the shelf
  • The very bottom has no shelves so you can remove the 4x waypoints and just connect the end to end.
  • The very top has no shelves until those 2 on the right, so you can remove the intermediate ones (4x on the left, 1x in the middle between the 2 shelves)
  • The top 2 small shelves look to have 4x shelf units on it, please set 4x waypoints in the center of eahc rather than the 2x we have today (I know they'll be tightly clustered - its actually a good test case)
  • Can we have the 2x large shelves on the left have pick locations in their centers too? If you have that route hug the shelf instead of the wall on the far left, then it can be dual purpose. I think we can skip adding nodes to the shelves creating the 'maze' at the lower left, but we should have nodes for each shelf otherwise. What we can do here is to keep the line down the middle that you have (bidirectional) and then branch off from that to the shelf centers in a straight line orthogonal to it (like a tree). We should have the 'normal' line down the outermost right shelf though as that doesn't have any larger obstructions than the other shelving unit. That should cover the 2x big shelves fully.
  • We should add in a second (or more?) connection in the graph from the units on the right we have covered and the units on the left so its not just a single edge that connects them. For example, it looks like if we extended 2x of the aisle lines on the right they would hit the shelf on the left without any obstructions. Once we have that route line in place, we can add in those nodes to make it a straight line and have additional connection points.
  • This is how the map looks currently:

    Screenshot from 2025-05-20 04-27-41

    I have introduced an additional node on spawn so that the robot can be moved in the example.

    PS: I have drawn the dots roughly by hand for illustration, the actual dots are aligned in a straight line.

Traversal

  • Change the MAP_TYPE to warehouse in the route_example_launch.py file:

    ros2 launch nav2_simple_commander route_example_launch.py
    nav2_route.mp4

    The traversal is from the 0th to the 61st node.

@leander-dsouza
Copy link
Contributor Author

Additional Changes

  • I have modified the example route launch example to have speed and keepout zones enabled to prevent throttling of the print messages in the terminal.

  • In addition, I have defined configurable start and goal positions for depot and warehouse, so that it works out of the box for both conditions.

@SteveMacenski
Copy link
Member

SteveMacenski commented May 20, 2025

The continuous logging from the speed / keepout filter when its not enabled is really, really annoying. Can we throttle that down to every 10 seconds with RCLCPP_WARN_THROTTLE?

I'd also like to find a way to stop it after awhile. Whether that's somehow using the use keepout / speed launch configurations to set the enabled to false for those filters when not in use to disable them on startup (optimal) or just stop trying after some timeout and automatically disabling itself. The hardest part of setting the parameter to false is that the keepout/speed zones could be named anything in the yaml file since they're plugins.

What I'm thinking is that we could set enabled to be false by default and then have the keepout/speed launch files remap them to true when in use. We could do that a couple of ways:

  • Add in a placeholder substitution in the yaml file for enable: that could be replaced, i.e. enabled: KEEPOUT_ENABLED
  • Have a configuration for the name of the speed/keepout filter so we know the exact path to its parameter, i.e. local_costmap.local_costmap.keepout_zone.enabled

The first seems like the best, since there's no long-parameter keying and we can immediately find any/all instances of it from multiple costmaps as long as they use the same substitution KEEPOUT_ENABLED (or whatever). If its enabled, we can substitute this to true else, false.

@SteveMacenski
Copy link
Member

On the graph: Love it! Nitpicks only now:

  • Can we make the bottom most aisle also directional to match the pattern?
  • The 'tree' on the left's nodes don't match the shelf centers for the left-most shelf (they use the same node as the right shelf which lines up and I think we need to add additional nodes for the left to match its centers)
  • The top right short shelves it looks like the upper-most nodes are not quite centered / are close to the collision area. Can we pull those down a little bit so they're not so near collision?
  • I assume for the shelf just below the robot those black dots missing is just an annotation issue and those nodes are actually present?
  • Thanks for adding the additional connections between the 2 halves of the graph. If we could add one more that would be great. The aisle just below the one with the robot has a <-- arrow that would connect up as well. That would be good to have one aisle that's directly pointed into that zone instead of away from it (the other 2x are -->).
  • A connection could exist from the bottom of the "tree" on the left with the lower connection in the center after the right angle. Extending the tree line down and the lower-most connector of the 2 graph halves to the left would make a full closed-loop around that center shelf.

For the final version of this, it would be good to have this graphic with the arrows in place, this is useful to show visually the policy. We should place this in the documentation somewhere and/or in the tutorials to show the example. This is useful context for how the graph works. Just fix the missing nodes too :-)

@leander-dsouza
Copy link
Contributor Author

On the graph: Love it! Nitpicks only now:

  • Can we make the bottom most aisle also directional to match the pattern?
  • The 'tree' on the left's nodes don't match the shelf centers for the left-most shelf (they use the same node as the right shelf which lines up and I think we need to add additional nodes for the left to match its centers)
  • The top right short shelves it looks like the upper-most nodes are not quite centered / are close to the collision area. Can we pull those down a little bit so they're not so near collision?
  • I assume for the shelf just below the robot those black dots missing is just an annotation issue and those nodes are actually present?
  • Thanks for adding the additional connections between the 2 halves of the graph. If we could add one more that would be great. The aisle just below the one with the robot has a <-- arrow that would connect up as well. That would be good to have one aisle that's directly pointed into that zone instead of away from it (the other 2x are -->).
  • A connection could exist from the bottom of the "tree" on the left with the lower connection in the center after the right angle. Extending the tree line down and the lower-most connector of the 2 graph halves to the left would make a full closed-loop around that center shelf.

For the final version of this, it would be good to have this graphic with the arrows in place, this is useful to show visually the policy. We should place this in the documentation somewhere and/or in the tutorials to show the example. This is useful context for how the graph works. Just fix the missing nodes too :-)

I have added all your suggestions, Steve. This is how the graph looks now with the arrows in place:

warehouse_illustration

@leander-dsouza
Copy link
Contributor Author

The continuous logging from the speed / keepout filter when its not enabled is really, really annoying. Can we throttle that down to every 10 seconds with RCLCPP_WARN_THROTTLE?

I'd also like to find a way to stop it after awhile. Whether that's somehow using the use keepout / speed launch configurations to set the enabled to false for those filters when not in use to disable them on startup (optimal) or just stop trying after some timeout and automatically disabling itself. The hardest part of setting the parameter to false is that the keepout/speed zones could be named anything in the yaml file since they're plugins.

What I'm thinking is that we could set enabled to be false by default and then have the keepout/speed launch files remap them to true when in use. We could do that a couple of ways:

  • Add in a placeholder substitution in the yaml file for enable: that could be replaced, i.e. enabled: KEEPOUT_ENABLED
  • Have a configuration for the name of the speed/keepout filter so we know the exact path to its parameter, i.e. local_costmap.local_costmap.keepout_zone.enabled

The first seems like the best, since there's no long-parameter keying and we can immediately find any/all instances of it from multiple costmaps as long as they use the same substitution KEEPOUT_ENABLED (or whatever). If its enabled, we can substitute this to true else, false.

I totally agree, I will make a PR addressing this soon. I will increase the throttling timeout to 10 seconds and make the launch file enable the filters selectively.

@SteveMacenski
Copy link
Member

Perfect! Can you open a PR on the migration guide mentioning that there are now graphs for the Warehouse with directional aisle lanes for example with this image? It might also be useful

On the configuration page, I would recommend writing up a few sentences about this (directional lanes set, otherwise all other edges are bidirectional, nodes are pick locations, tool used to generate it, etc).

I'm noticing that the robot doesn't look like it actually starts at that first node location. I'm looking at the laser scan and it looks like the wall behind the robot and the pallet in front of the robot have their laser scans off by ~1.5-2m. That imples the initial localization pose of the robot is incorrect and then the first node should be moved respectively with where the robot actually spawns. After that's fixed, I can merge this in!

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
@leander-dsouza
Copy link
Contributor Author

I'm noticing that the robot doesn't look like it actually starts at that first node location. I'm looking at the laser scan and it looks like the wall behind the robot and the pallet in front of the robot have their laser scans off by ~1.5-2m. That imples the initial localization pose of the robot is incorrect and then the first node should be moved respectively with where the robot actually spawns. After that's fixed, I can merge this in!

Amazing catch!
I have moved the warehouse spawn location to match the starting node.

Screenshot from 2025-05-21 00-59-35

@SteveMacenski
Copy link
Member

Fabulous, code is approved! Just add in the docs to highlight this great work and we're good to go :-)

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.

Just one more PR I want in before the Kilted branch now!

@leander-dsouza
Copy link
Contributor Author

Fabulous, code is approved! Just add in the docs to highlight this great work and we're good to go :-)

Yes, sure. I will make the addition to the docs shortly.

@leander-dsouza
Copy link
Contributor Author

I have updated the documentation to include additional resources for this change over at #696.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
@SteveMacenski
Copy link
Member

SteveMacenski commented May 21, 2025

OK!

BTW: I started thinking about the KEEPOUT_ZONE_ENABLED and SPEED_ZONE_ENABLED param substitutions and I think what makes sense is to extend the RewrittenYaml function to also have a field for search and replacing keys or values

class RewrittenYaml(launch.Substitution): # type: ignore[misc]
. We can give it {'SPEED_ZONE_ENABLED': True, ...} as a map and have it iterate through all keys and values. if any matches, then we replace that key or value with the True. I tried some other directions and they didn't work out using ROS 2 direct tooling, so I think that's the right direction

@leander-dsouza
Copy link
Contributor Author

class RewrittenYaml(launch.Substitution): # type: ignore[misc]

. We can give it {'SPEED_ZONE_ENABLED': True, ...} as a map and have it iterate through all keys and values. if any matches, then we replace that key or value with the True. I tried some other directions and they didn't work out using ROS 2 direct tooling, so I think that's the right direction

This makes perfect sense!
I can implement this soon in an upcoming PR if you are not working on this already, Steve :)

@SteveMacenski
Copy link
Member

I stopped at the moment, so all you!

@SteveMacenski SteveMacenski merged commit c7589c9 into ros-navigation:main May 23, 2025
14 checks passed
stevedanomodolor pushed a commit to stevedanomodolor/navigation2 that referenced this pull request May 27, 2025
* Added routes for warehouse map.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Renamed routes for the depot warehouse to match naming convention.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Rename turtlebot4 geojson graphs to remove the turtlebot4 prefix.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Restructured warehouse_graph to enable bidirectional navigation.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Improved connectivity of the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added keepout and speed zones to the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added flexibility in setting goals for the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Move euler to quaternion function to utils.py.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added additional routes to the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Shift warehouse spawn location to match start of graph node.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added parameters to control enabling of costmap filters.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added node information to the route example launch file.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
stevedanomodolor pushed a commit to stevedanomodolor/navigation2 that referenced this pull request May 28, 2025
* Added routes for warehouse map.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Renamed routes for the depot warehouse to match naming convention.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Rename turtlebot4 geojson graphs to remove the turtlebot4 prefix.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Restructured warehouse_graph to enable bidirectional navigation.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Improved connectivity of the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added keepout and speed zones to the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added flexibility in setting goals for the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Move euler to quaternion function to utils.py.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added additional routes to the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Shift warehouse spawn location to match start of graph node.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added parameters to control enabling of costmap filters.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added node information to the route example launch file.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
SakshayMahna pushed a commit to SakshayMahna/navigation2 that referenced this pull request Jun 8, 2025
* Added routes for warehouse map.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Renamed routes for the depot warehouse to match naming convention.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Rename turtlebot4 geojson graphs to remove the turtlebot4 prefix.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Restructured warehouse_graph to enable bidirectional navigation.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Improved connectivity of the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added keepout and speed zones to the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added flexibility in setting goals for the route example.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Move euler to quaternion function to utils.py.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added additional routes to the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Shift warehouse spawn location to match start of graph node.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added parameters to control enabling of costmap filters.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added node information to the route example launch file.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

2 participants