-
Notifications
You must be signed in to change notification settings - Fork 1.7k
Add warehouse routes for nav2 route server #5160
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
Add warehouse routes for nav2 route server #5160
Conversation
Codecov ReportAll modified and coverable lines are covered by tests ✅ see 8 files with indirect coverage changes 🚀 New features to boost your workflow:
|
| 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') |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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" |
There was a problem hiding this comment.
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 :-)
There was a problem hiding this comment.
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.
|
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: Note:
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! |
Thank you for the kind words, Steve :) |
7a3f905 to
1e191b7
Compare
I have updated the warehouse as follows. All arrows that are not marked are bidirectional: This is the warehouse world for reference: |
|
I am not sure as to why the |
|
Looks like a networking problem. I retriggered and its solved now. Last couple of changes:
Where does the robot spawn? We may want to add in a node around there if its too too far from the graph. |
Traversal
|
Additional Changes
|
|
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 What I'm thinking is that we could set
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 |
|
On the graph: Love it! Nitpicks only now:
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 :-) |
deffffc to
5eed091
Compare
I have added all your suggestions, Steve. This is how the graph looks now with the arrows in place: |
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. |
|
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>
5eed091 to
6683ade
Compare
Amazing catch! |
|
Fabulous, code is approved! Just add in the docs to highlight this great work and we're good to go :-) |
SteveMacenski
left a comment
There was a problem hiding this 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!
Yes, sure. I will make the addition to the docs shortly. |
|
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>
|
OK! BTW: I started thinking about the
{'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 stopped at the moment, so all you! |
* 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>
* 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>
* 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>






Basic Info
Description of contribution in a few bullet points
.geojsongraphs so that the map type can be replaced for depot/warehouse.route_example_launch.py.Description of documentation updates required from your changes
Description of how this change was tested
The graph was tested for traversability using the below command:
warehouse_route.mp4
Future work that may be required in bullet points
For Maintainers: