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 office.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.

[Yadunund]

Make std::variant<rmf_dispenser_msgs::DispenserRequest, rmf_ingestor_msgs::IngestorRequest> and rename to Destination?

Then we have separate capabilities to construct DispenserRequest and IngestorRequest from string and return a vector needed by the DispatchRmfRequest skill.

[luca-della-vedova]

Renamed 18070e6.
I gave some thought to the DispenserRequest / IngestorRequest usage and I'm not 100% sure it is a good idea.

These structures are messages and using them would send the message (hah!) that they will be used in the stack, perhaps to be published.
The reality though, is that the main usage of this struct is to generate the task api request message, and the only use we have for dispenser requests is subscribing to them to tell the workcell that the robot arrived. We could do a separate structure and something like std::variant<Pickup, Dropoff>, where:

struct Pickup {
    std::string workcell;
}
// Same for dropoff

I would actually only implement Pickup in a first iteration since there is really no support for dropoffs in the current architecture so we can't even design a test to make sure they work. Hopefully though a std::variant<Pickup> with a well documented TODO to add more variants can be a good future extension point?

[Yadunund]

These structures are messages and using them would send the message (hah!) that they will be used in the stack, perhaps to be published.

Hmm I'm not sure if I agree with that. It's a data structure that we can reuse and avoid extra conversions between a class object and the message later on? Besides, all of this stuff is purely internal and we'll be relying on skills like ExtractDestination and UnpackDestinationData to create and parse the data structure.

[Yadunund]

Let's use deliveryRobot from RMF aka MiR.

[luca-della-vedova]

@aaronchongth for the next iteration on sim world

[Yadunund]

Double check if we need this.

[luca-della-vedova]

Actually yes! This is to execute this behavior tree node

nexus/nexus_integration_tests/config/system_bts/main.xml

Line 8 in c96fdbf

<DispatchTransporter name="bid_amr_transporter" result="{amr_transporter}" transport_task="{transport_task}"/>

What this system orchestrator node does is:

• Iterate over tasks in the work order
• Generate a transportation task for the current work order that visits all the workcells that have been assigned to each task.
• Bid this new transportation request to all the registered workcells. Fail if no workcell can do this transportation request.
• Output the transportation task as well as workcell that was assigned to it.

There is admittedly a bit of duplication in the bidding here and the bidding at the system orchestrator level. The problem that needs to be solved is that this specific task needs to have full knowledge of the result of the workcell task assignment to know where to send the AMRs, so it needs to run in a following step

[luca-della-vedova]

It could be renamed to something a bit clearer though, maybe DispatchTransportationTask? Or split into a GenerateTransportTask that only generates a task to visit all workcells and DispatchTask that takes it as an input and does the bidding / assignment

[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]

I don't think we should clone rmf_demos here. If we need the tinyRobot model, suggest committing it to Fuel with the slotcar plugin. See my comment above on committing the .world and nav_graph files. We can also commit any launch files we need or better if we can rely on a condition within control_center_launch.py to start RMF related nodes instead of the legacy transporter.

[luca-della-vedova]

Apart from the launch files that I guess we can duplicate, we need rmf_demos_fleet_adapter to manage the slotcar robots. Unless we copy it in (!) or create a much more bare simulation method we can't really get away from having rmf_demos cloned.
If we distributed at least rmf_demos_fleet_adapter on the buildfarm we could remove this dependency.

[luca-della-vedova]

Or we could also do the nuclear option of not using the rmf_demos_fleet_adapter but using the very old C++ full_control fleet adapter but that's also not a great idea imo

[luca-della-vedova]

What I have done for now is still clone it (since we need the fleet adapter) but remove all other rmf_demos dependencies.
Since the CI should do a build "up to" nexus_integration_tests the total impact should be only building one Python package which is negligible.
I am not sure a better option exists, other than writing a full mock fleet adapter with a mock simulation, using legacy fleet adapters or distributing binaries of the demo fleet adapter, as mentioned above.

[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)