Introduce a RMF transportation workcell

[luca-della-vedova]

This PR introduces RMF integration into nexus, where RMF is a workcell, managed by the workcell orchestrator, that is capable of executing transportation tasks through a new behavior tree and set of capabilities.

This is in a simple demo stage. I brought in a modified office world, with the only modifications being renaming the dispensers to the names of the workcells, and added a new launch to nexus_integration_tests that launches RMF together with Nexus, as well as changed the movement of items to be based on an AMR rather than a mock transporter.

Test it!

Clone, build and run:

ros2 launch nexus_integration_tests depot.launch.xml headless:=false

Submit a task:

ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {id: '23', work_order: '$(cat config/pick_and_place.json)'}}"

You should see the transportation happening:

Screencast.from.2024-12-12.18-10-14.webm

PR breakdown

The PR is large but I'll try to condense the main decision (and potentially controversial) points I went through during the design.

nexus_integration_tests vs nexus_demos

It would be more natural to create a new nexus_demos package that contains the bringup and I got halfway there before realising it would make the diff explode even further, so I went for an initial approach that reduces the number of changes in nexus_integration_tests, we can then do a followup PR that splits the package into a nexus_demos and a nexus_integration_tests, or maybe just rename it.

Task cancellation

As noted in #40, the cancellation behavior of the workcell can't be customized and defaults to letting tasks run to completion. This means that the RMF task will not be cancelled and if a robot happens to be halfway through a long task and be waiting for a workcell that is cancelled, it will hang its waiting indefinitely. Once #40 is addressed we should add task cancellation to the TransportAmr capability.

Is task doable / navgraph checking

As noted in #41, the payload can't be used for verifying task capability. Transportation tasks have a payload with a list of destinations and they will currently always return true regardless of whether the destinations exist or not. A more advanced capability checking that, for example, checks the fleet's navgraph for existence of waypoints, would be a better design.

Map annotation

Visualizing the workcell requires its position to be populated, however Nexus (and the workcell orchestrator) currently have no way to populate this information.
For now just for the sake of visualizing I wrote a node that subscribes to the /map topic and looks for all waypoints with the pickup_dispenser property and use their location to populate markers. It will then subscribe to states and update them.

A better long term design would involve passing the workcell orchestrator information about the location of the workcell, pass it to the system orchestrator when registering and refactoring the visualization node to regularly calls the /list_workcells service to query for existence of new workcells. I deferred this to avoid adding a large diff to the workcell orchestrator node and keep changes strictly addictive for review simplicity.

Signaling

I introduced the capability of receiving signals for the system orchestrator, as well as change the default behavior tree to wait for the AMR before starting the workcell, rather than halfway. This was done to improve reliability in case of parallel tasks (i.e. there is no risk of a workcell starting a task, just for the wrong AMR to come in) but parallel tasks are still not quite there so not sure if it is still needed. An example of behavior tree that implements this new logic is here.

What's next

Many things! But this PR is already at a very large size and I tried to keep the diff minimal (where I liberally define "diff" as pre-existing files that are changed and risk breaking existing behavior, not new additions that are more likely to be safe).

Create a Gazebo simulation that includes workcells together with AMRs

Right now the workcells are not simulated in Gazebo, it would be great to have a proper simulation world so users can inspect what is happening.
Often these workcells have conveyor belts to feed the items to / from the AMRs, these would also be valuable additions.

Simulate humans for workcells that are manually operated

In real life, not all workcells are automated and some are just operated by humans. We could mock this in simulation by just having a human in the dropoff point and a special behavior tree that just waits for an input.

Task parallelism

Currently submitting parallel tasks can risk deadlocking the system, since RMF and Nexus are somewhat independent. We should revisit the implementation to make sure we can have parallel tasks.

SKU Tracking

It would be interesting to show the position and status of the SKUs in rviz. This is especially useful to know their state as they are being moved throughout the facility.

Better handling of workcell location and registration

As noted in the Map annotation section of the PR description, populate the information at workcell registration time and not by subscribing to a /map topic.

Post processing of waypoints for AMR tasks

Currently, whenever a work order is received, an AMR task that goes through all the workcells will be generated and each workcell will only be signaled to start when the AMR arrives.
This however, will be suboptimal in two corner cases:

1. If there is only one workcell and we don't want to use an AMR to transport, we will still request an AMR to the location which is unnecessary.
2. If there are multiple tasks being done by the same workcell, the AMR will have multiple "pickup" phases, although I believe this should be innocuous and just introduce some extra signaling.

It is actually a bit tricky to design a single behavior tree that works for all cases and I would actually suggest using a different behavior tree for different purposes, such as the first case.

[luca-della-vedova]

With 2ddcafa I now fully duplicated the behavior trees between the old transporter based implementation and the new RMF based implementation. I also duplicated the integration test so that both are now part of our test suite.

The diff now should be much more "purely additive" and easier to review

[Yadunund]

Is task doable / navgraph checking
As noted in #41, the payload can't be used for verifying task capability. Transportation tasks have a payload with a list of destinations and they will currently always return true regardless of whether the destinations exist or not. A more advanced capability checking that, for example, checks the fleet's navgraph for existence of waypoints, would be a better design.

With #41 merged, we could write a new RMFTaskChecker plugin that creates a bunch of rmf_traffic::Graph() objects based on input nav_graph paths and report whether a destination is connected to any graph. This can be a separate PR but let's open a ticket.

[Yadunund]

Starting to look closer into the implementation. Sharing a big feedback to address while I review the capabilities.
Let's not commit the various files needed to generate the office world and navgraphs and do so at compile time. Instead let's directly commit the .world and nav_graph files generated. Let's also throw all rmf related files inside nexus_integration_tests/config/rmf/.

[Yadunund]

I'd really prefer committing the .world and nav_graph files needed to run the test as opposed to the usual RMF way of generating these files at build time. We can throw them inside config/rmf/ along with anything else needed.
The world file just needs to be an empty floor with a couple robots included. Even better if the robot model URIs point to fuel.

We can follow the usual style when designing nexus_demos.

[luca-della-vedova]

Committed under config/rmf in 161b2a4, as well as vendored launch files so we don't depend on rmf_demos, rmf_demos_gz or rmf_building_map_tools.

[Yadunund]

Feedback on workcell visualizer.

[luca-della-vedova]

Starting to look closer into the implementation. Sharing a big feedback to address while I review the capabilities. Let's not commit the various files needed to generate the office world and navgraphs and do so at compile time. Instead let's directly commit the .world and nav_graph files generated. Let's also throw all rmf related files inside nexus_integration_tests/config/rmf/.

Note that we will still need both the building and the navgraph if we want to run simulation so will likely have to still commit both original map and generated files.
The reason is that the simulated robots use the map published by the building_map_server to find what level they are in.

[luca-della-vedova]

I addressed the feedback on the visualization node and spun it out into another PR #57, marked as draft since it should only be merged after this (and probably after demos as well)

[luca-della-vedova]

In f55bab2 I reverted the signaling at the system orchestrator level.
Now the workcell behavior trees are exactly the same regardless of whether it is a pick and place on a conveyor or on an AMR.
Sadly remapping gets in the way since we need to make sure we run the same workcell behavior tree but a different system orchestrator behavior tree (and not only the main.xml, but also the one that is loaded, pick_and_place.xml).

Furthermore, I explored the idea of removing all the duplicated behavior trees / work orders altogether in f9705a7. The idea is that if we just expose the remap_task_types parameter to the launch files we can use it to make sure the same work order results in a different system orchestrator behavior tree but the same workcell behavior tree. But happy to revert it if this is not desirable