Fleet adapter publishes its navigation graph

[Yadunund]

This PR

• Adds APIs to rmf_traffic_ros2/Graph.hpp to allow conversions between rmf_traffic::agv::Graph and rmf_building_map_msgs::Graph objects while preserving all waypoint and lane properties
• Enables the fleet adapter to publish its navigation graph as a building_map_msgs/Graph msg over the /nav_graphs topic. The QoS is set to durability=transient_local, reliability=reliable.

This is meant to be a static graph. For each waypoint, properties such as map_name, is_holding_point, is_charger etc are appended to the Params[] field in GraphNode.

Note: The status of each lane, ie, whether open or closed along with lane properties like speed limits will be published over a separate topic. This will be implemented in a separate PR #217

######################################
The original description included this question:
On a side note, I tried using the message::build pattern as seen below

rmf_building_map_msgs::msg::GraphNode node;
rmf_building_map_msgs::msg::Param param;
...
...
node.params.emplace_back(
    rmf_building_map_msgs::build<rmf_building_map_msgs::msg::Param>()
    .name("is_holding_point")
    .type(param.TYPE_BOOL)
    .value_int(0)
    .value_float(0.0)
    .value_string("")
    .value_bool(false)
);

but the compiler throws the error:

error: use of ‘auto rmf_building_map_msgs::build() [with MessageType = rmf_building_map_msgs::msg::Param_<std::allocator<void> >]’ before deduction of ‘auto’

Out of curiosity, what was I doing wrong?

######################################
The solution is to explicitly import the message header file.

Signed-off-by: Yadunund yadunund@openrobotics.org

[codecov]

Codecov Report

Merging #207 (2e51e13) into main (3be4edf) will decrease coverage by 0.24%.
The diff coverage is n/a.

❗ Current head 2e51e13 differs from pull request most recent head 3f568b9. Consider uploading reports for the commit 3f568b9 to get more accurate results

@@            Coverage Diff             @@
##             main     #207      +/-   ##
==========================================
- Coverage   17.61%   17.37%   -0.25%     
==========================================
  Files         209      836     +627     
  Lines       19351    79008   +59657     
  Branches     9297    38456   +29159     
==========================================
+ Hits         3409    13724   +10315     
- Misses      12274    50263   +37989     
- Partials     3668    15021   +11353     

Flag	Coverage Δ
tests	17.37% <ø> (-0.25%)	⬇️

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

Impacted Files	Coverage Δ
..._monitor_nodes/delayed_query_broadcast_monitor.cpp
...f_fleet_adapter/src/read_only/FleetAdapterNode.hpp
.../rmf_fleet_adapter/test/test_read_only_adapter.cpp
...rc/rmf_fleet_adapter/jobs/detail/impl_Planning.hpp
...pp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-notification.hpp
...apter/src/rmf_fleet_adapter/jobs/SearchForPath.cpp
...mock_participants/repetitive_delay_participant.cpp
...r/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
...f_fleet_adapter/services/FindEmergencyPullover.cpp
...2/rmf_fleet_adapter/rmf_rxcpp/test/test_RxJobs.cpp
... and 1035 more

[mxgrey]

Out of curiosity, what was I doing wrong?

Try using .push_back(...) instead of .emplace_back(...). The function emplace_back does "perfect forwarding" which means it needs to infer the type that you're meaning to pass in. Sometimes that doesn't work great. I'm not entirely sure if that's actually the issue because I've never seen that particular error before, but that would be my best guess.

[Yadunund]

Turns out the error was caused because I did not explicitly include the message header files for the nested messages within rmf_building_map_msgs::msg::Graph. Adding those in fixed the problem ad9ee11

This should be good for a review now.

[Yadunund]

@mxgrey I think it would be better if there is an external library that offers APIs to convert from rmf_traffic::agv::Graph to rmf_building_map_msgs::msg::Graph and vice versa. That way there is some consistency in how the params are populated/converted. The implementation here can call the method to convert a rmf_traffic::agv::Graph to rmf_building_map_msgs::msg::Graph and publish it out. Then downstream applications such as lane modifiers can use the API to obtain back the rmf_traffic::agv::Graph from the received rmf_building_map_msgs::msg::Graph using the same conversion library.

Does this sound good? If so shall I add these methods to rmf_traffic_ros2? https://github.com/open-rmf/rmf_ros2/blob/main/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp

Or should I create a separate package within rmf_ros2 if we don't want rmf_traffic_ros2 to depend on rmf_building_map_msgs.

[mxgrey]

I think adding reusable serialization and deserialization functions to a library is a great idea, so big 👍 to that.

Adding those functions to rmf_traffic_ros2 sounds perfect to me as well.

I took a moment to consider whether the graph message belongs in rmf_traffic_msgs instead of rmf_building_map_msgs so that there's better symmetry between the rmf_traffic objects and their rmf_traffic_msgs serializations, but I haven't come to any strong conclusions on that. On one hand, it makes sense for the graph message to go into rmf_traffic_msgs because it's the serialization of a data structure that's from rmf_traffic. On the other hand, virtually everything else in rmf_traffic_msgs is specific to the traffic scheduling and negotiation system, which is not really the case for the navigation graph.

In conclusion, I see nothing wrong with keeping the graph message in rmf_building_map_msgs, especially to minimize the disruptiveness of the change.

[Yadunund]

Thanks for the quick feedback!

In conclusion, I see nothing wrong with keeping the graph message in rmf_building_map_msgs, especially to minimize the disruptiveness of the change.

Roger 👍🏼

[Yadunund]

@mxgrey I've updated the implementation as discussed.

It's not apparent to me how we can serialise anLane::EventPtr to determine whether there is a door/lift open/close event or dock/wait event.
Similarly with OrientationConstraints. So I've left a note in the API documentation highlighting this limitation.

[luca-della-vedova]

Just a high level comment since I worked a bit on the linked graph conversion function.
Something that I was worried about when doing that implementation was code duplication with the existing parse_graph. While it could not really be helped in that case (we were parsing a brand new file format, that doesn't have vertex indexes, only works in WGS84 coordinates, doesn't really support multilevel buildings) I feel it could be avoided in this case.

From what I understand the function under parse_graph parses a string which is a yaml navgraph and converts it into a rmf_traffic::agv::Graph while the function you wrote does bidirectional conversion between the rmf_building_map_msgs::Graph and the rmf_traffic::agv::Graph.
I feel there aren't huge differences between parsing a raw yaml and the rmf_building_map_msgs and now we have yet another graph parser, yaml, rmf_site_map_msgs and rmf_building_map_msgs with almost (but not full) feature parity, it's not great to have to change three separate parsers whenever a change in file format is introduced (i.e. a new property to the graph)

• Would it be possible to deprecate at least one? A possible idea could be that the fleet adapter subscribes to the whole building map, finds its own graph and republishes it for downstream users, in this way we can deprecate parse_graph and only use the new rmf_building_map_msgs parsers.
• An alternative, especially if you want the publishing to be static and done once at running time, could be to have this done by the building_map_server but I guess the idea is that fleet adapters are in charge of their own navgraph?
• Another alternative could be to have an approach similar to what is done for the rmf_site_map_msgs, where the whole nav graph is just published as a string, then if parsing (i.e. parse_graph) was part of a shared library it can be used among all nodes that need to process it.

My personal favorite is the first option, but of course I'm biased because I was an advocate of the dynamic navgraph approach in the past 😅 .
The building map server already publishes a navgraph when it publishes a map, specifically for each level it publishes a navgraph, it could require a bit of tuning to make sure all the properties are there (for example on a quick glance I couldn't see support for multiple coordinate systems) but the code path to generate the message and the navgraph yaml is the same (they both call this function) so most features should be there.
Now since the properties in the message served by building_map_server and the properties in the static yaml are the same, we should be able to pretty much copy the whole parsing logic from parse_graph (and write the serialization) to have full feature parity, including orientation constraints, doors and lift events.

The biggest drawback is that now the map server has to be running for fleet adapters to be initialized which is an additional dependency and I don't know what the implications might be. On the other hand, we would be able to get rid of the dependency in fleet adapters for nav graph files (i.e. here) which would now become just an index and I can envision in the future we could have a naming system in graphs so that instead of using 0-9 integers we use the fleet name, that would get rid of the navgraph parameter altogether.

What do you think?

[Yadunund]

@luca-della-vedova I think you've misunderstood the functionality here. We're not converting between the rmf_building_map_msgs::BuildingMap message published by the rmf_building_map_server and the rmf_traffic::agv::Graph object.

We're adding a utility to publish the exact navigation graph used by the fleet adapter for its planning. For convenience, we're re-using the rmf_building_map_msgs::Graph message definition.

The biggest drawback is that now the map server has to be running for fleet adapters to be initialized which is an additional dependency and I don't know what the implications might be.

There is no subscription anywhere in this implementation to the building map server and hence no dependency.

We've had this discussion before and we agreed that it is best if the fleet adapters are the ones to publish the exact nav graph they are using. The building map server has no way to know whether a fleet is actually configured to use a nav graph of certain index. Even if we update some definitions to map the index to the fleet name, there is no guarantee that the fleet adapter was actually loaded with this navgraph. Maybe they don't even use the exported nav_graph.yaml.

[Yadunund]

Commit f936da7 ensures that all lane events (DoorOpen, DoorClose, LiftSessionBegin, LiftDoorOpen, LiftMove, LiftSessionEnd) and orientation constraints are serialized/deserialized.

@mxgrey this feature is complete and ready for review.

[mxgrey]

I think it's fine to merge, but I do have one remark in an inline comment.

[Yadunund]

Could I get a re-approval for this?