[Re-write] Free fleet adapter using easy-full-control fleet adapter and zenoh bridges

[aaronchongth]

New feature implementation

A large motivation of this re-write, is to fully utilize the well tested tools already available in the ROS ecosystem and Zenoh, and also take advantage of python bindings as much as possible, to allow for easier iterations, testing and contributions.

The use of Zenoh bridges will hopefully also minimize disruption to any existing robot navigation stack, and can be spun up with just a standalone binary with the appropriate configuration file.

The legacy implementation can still be found at the legacy branch.

• re-implemented free_fleet using easy_full_control, based off the fleet_adapter_template, will allow setting up of perform actions, Support perform_action feature of the fleet adapter #116, [Feature request]: Add /robot_state topic published from free_fleet_server #141
• fleet adapter to use zenoh as the main way of communicating with robots
• change approach to use zenoh-bridge-ros2dds as a drop-in method of bridging required ROS 2 topics and actions between robot and fleet adapter
• supports the nav2 stack with the simple /navigation_to_pose action, ROS 2 navigation stack client #84
• using pycdr2 to serialize and deserialize messages, with required message definitions fleshed out, Question on message generation #129
• examples using existing simulations from nav2_bringup
• updated README, https://github.com/open-rmf/free_fleet/blob/easy-full-control/README.md
• supports Jazzy (other distros not tested yet), What are the plans on moving to humble (Ubuntu 22.04)? #138
• linting, Add style linting #100

Architecture

This architecture and the use of Zenoh bridges will ideally improve the integration experience, with just setting up the zenoh-bridge-ros2dds and its configuration, instead of building and running a separate node.

This PR does not support the ROS 1 navigation stack, the diagram is just for illustrating the proposed architecture once it is implemented down the road.

Single robot example using official nav2_bringup's tb3_simulation

Two robots example using official nav2_bringup's unique_multi_tb3_simulation

Although the two robot simulation example is just for testing purposes as it uses a different architecture as intended

Next TODOs (over subsequent PRs)

• attempt to optimize tf messages (not all are needed)
• ROS 1 nav support
• map switching support
• docking support
• support for Rolling
• docker images
• releases
• testing and support for other RMW implementations

There are plenty of TODOs left, but this is the bare minimum testable and usable state

[xiyuoh]

Thank you for revamping free fleet! Tried out the examples and things are looking fantastic. I was able to bring up both single and multiple TB3 tests by following the instructions on README.

Left some minor comments for the README that are mostly about typos and clarity. For the fleet/robot adapter, I was primarily looking out for edge cases where self.nav_goal_id might fall out of sync with self.execution and catching any scenarios where the execution object is left unattended if self.nav_goal_id is set to None. This may lead to RMF not sending any subsequent commands, or sending outdated nav commands if replans are not being called prompty.

[PVijayaGanesh]

Hi I did Free fleet adapter using easy-full-control fleet adapter and zenoh bridges
Steps I follow:

1. Install humble rmf

2. clone the branch easy full controll free fleet ( git clone -b easy-full-controll <> )

3. zenoh v11 setup i did ..

4. ros2 run free_fleet_examples test_navigate_to_pose.py
   --frame-id map
   --namespace turtlebot3_1
   -x 1.808
   -y 0.503

   This command is working... i sends the goal and robot is moving

   5. when the launch free fleet adapter i got this issue..

How can i resolve this...

@xiyuoh @aaronchongth

[codecov]

Codecov Report

Attention: Patch coverage is 61.99525% with 160 lines in your changes missing coverage. Please review.

Please upload report for BASE (main@0909836). Learn more about missing BASE report.

Files with missing lines	Patch %	Lines
..._fleet_adapter/free_fleet_adapter/fleet_adapter.py	0.00%	112 Missing ⚠️
...t_adapter/free_fleet_adapter/nav2_robot_adapter.py	71.59%	42 Missing and 6 partials ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main     #145   +/-   ##
=======================================
  Coverage        ?   61.99%           
=======================================
  Files           ?        5           
  Lines           ?      421           
  Branches        ?       54           
=======================================
  Hits            ?      261           
  Misses          ?      154           
  Partials        ?        6           

Flag	Coverage Δ
tests	61.99% <61.99%> (?)

Flags with carried forward coverage won't be shown. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[aaronchongth]

Heads up @xiyuoh when you try it out again

While setting up static and integration tests, I've found that setting up a zenoh router between the bridges and the free_fleet_adapter provides the best stability and repeatability (IRL and in CI). Folks can reference the docker compose file.

I've updated the documentation, tests and architecture diagrams.

This is not a big change as it was already expected that folks will need to use a zenoh router eventually when setting up everything as a distributed system.

[xiyuoh]

I believe this case is handled under stop(), so we can probably leave CANCELED scenarios out. When RMF requests for a robot to stop, leading to all goal requests being canceled, self.execution and self.nav_goal_id are already set to None inside the stop() and _handle_stop_navigation() callbacks respectively. On the other hand, is_navigation_done() is only ever called when both self.execution and self.nav_goal_id are not None, so this case would be redundant here.

Suggesting to leave it out because it'd be good to have a single source of truth/logic for goal canceled cases, especially when we are returning True and telling RMF that we have successfully completed a navigation task. Feel free to let me know if I'm misunderstanding the usage.

[aaronchongth]

So far the case doesn't do anything other than tell the robot adapter that navigation is done, there was a race condition I encountered while testing this behavior, that was "fixed" with 9cf376a#diff-6f7a8ca807eab341270b8ed8d8073dcea66db64ff693dd774d681225f2f526ab

Currently this code block doesn't do anything other than produce a log, and report that navigation is done. The alternative would be more hairy, as it will start going into the logic for replanning.

[xiyuoh]

I assume the intention of using an abstract class RobotAdapter is to open up options for integrators who want to tweak the implementation of the Nav2 adapter according to their use case, e.g. adding custom actions in the future.
In that case, some of these parameters can be ported into RobotAdapter to ensure that they will be initialized instead of relying on integrators to set them and possibly missing them out. It could also be useful to document this intention inside README to direct integrators to use this abstract class.

This also looks good to brought into the fleet_adapter_template with some tweaks, but that's a goal for another day.

[aaronchongth]

I would actually leave the setting of member variables to the child classes, in this case Nav2RobotAdapter, instead of moving these operations upward to RobotAdapter. Keeping RobotAdapter as a pure abstract class will help us not limit the constructors of any implemented RobotAdapter, but still maintain an overridden API that the final free fleet adapter can call upon

I believe this will help us when we start implementing the Nav1RobotAdapter. But in any case, if I don't see any major benefit down the road, I'll follow your advice and help move these upwards into RobotAdapter and update fleet_adapter_template too

[xiyuoh]

Gotcha, I understand where you're coming from. Flexibility is definitely a desirable trait we want in these classes. Regarding member variables, params such as update_handle, fleet_handle and node (and maybe even robot name) are pretty much non-negotiable for any EFC fleet adapter. Having them defined in the abstract class would ensure that users provide them as arguments when setting up their fleet adapters. The alternate case could be users sifting through existing fleet adapter implementations to pick out variables they assume are important, and potentially leaving out these "essentials", making the setup process longer and more confusing. I'm not leaning very strongly to have these added in though, just a suggestion.

If you're reconsidering including them but are still worried about constructor limitations, I recently received a review comment that suggested wrapping variables in an ActionContext object, and import it into the abstract class as a single argument. I thought the concept could be useful for this scenario too, allowing us to update the Context object without breaking API if we ever want to add on to the non-nego variables.

With that said, I still struggle to see the need for a pure abstract RobotAdapter class that only lives within free_fleet when it has nothing specific to free fleet, and instead can be utilized across all types of EFC fleet adapters for different robots. Imo it would be more suitable to port this to the fleet adapter template, and have free fleet adapters inherit the class from there. That way we won't have multiple abstract class across all our fleet adapter repos. If we do want an abstract RobotAdapter class under the free_fleet repo, it should probably contain some free_fleet-specific methods.

[aaronchongth]

Ok, there are a few dilemmas here, because the RobotAdapter is for testability and consistency over the nav1 and nav2 robot adapters (and any other types of navigation stack we might consider in the future), and not a plugin system,

potentially leaving out these "essentials"

the essentials can technically be left out if their fleet adapter implementations don't require it (I can't think of why, but it is possible to leave out name and node, and just have the fleet adapter keep track of it)

users might also want to create their own node inside their own RobotAdapter perhaps to set up different callback groups for their own uses or optimizations, instead of using the provided node like we usually do

If you're reconsidering including them but are still worried about constructor limitations

Since the next few items on the list for free_fleet is related to implementing the nav1 robot adapter, what exactly can be abstracted into the constructor RobotAdapter is yet to be confirmed, and will have a high risk of changing, breaking API. I will look into using ActionContext, but I'd say we can leave that for later

the need for a pure abstract RobotAdapter class that only lives within free_fleet when it has nothing specific to free fleet

The Nav1RobotAdapter will be implemented in the same manner such that in the future, the free_fleet_adapter is able to control a mixed fleet of Nav1 and Nav2 robots, so I'd say it is rather related to free fleet, although as you said, not specific only to free fleet.

On the other hand fleet_adapter_template at the moment is a template, and the final implementation of any fleet adapter based on the template still needs to be written in a separate package, without importing it from fleet_adapter_template. Adding an abstracted RobotAdapter to fleet_adapter_template, although useful as a reference, could lead to even more confusion, as folks can import RobotAdapter but implement their own fleet adapter based on the fleet_adapter_template at that time . But if any implementation details (member variable name) changes upstream for the imported RobotAdapter, without updating their own fleet adapter, things will break. Hence I think it will be clearer to users if they have full visibility of RobotAdapter.

edit: adding link, 3d34315
I've moved the member variables directly used by the free_fleet_adapter into RobotAdapter, while keeping everything else API calls (which have been abstracted). I'd say only if we can make the upstream fleet adapter template a library or a plugin system for robot adapters, should we consider moving RobotAdapter upstream

[xiyuoh]

After discussing more about the abstract class with Aaron, free fleet seems like a good place to house it since the goal is for a single fleet to integrate with multiple types of robot adapters. It would be extremely useful for future maintainers and community members to contribute their own robot adapters with other nav stacks.

However, since the current iteration of the class is meant for testability and is subjected to change after experimenting with nav1, I suggested it might be more appropriate to flesh out the class methods, and only introduce it after we ensure that there will be no breaking changes (especially with nav1).

Apart from that this PR looks good to me, have tested it out in sim + witnessed the hardware working well with the fleet adapter. Thanks for this massive refactor!