Adds a simple parking spot management system.

[arjo129]

New feature implementation

Implemented feature

This commit adds a stupidly simple parking spot system. It ensures the
next location a robot goes to is not occupied. It also publishes
currently free parking spots. When combined with #308 it allows a robot
to move to the nearest free spot instead of the nearest spot only.

note: This PR still needs more testing

Depends on:

• Supercedes #63: Messages for proto-reservation system  rmf_internal_msgs#64

Known issues:

• When the robots start in simulation not all robots seem to be running
  the idle task, hence some robot dont end up claiming parking spots.

[luca-della-vedova]

I got it to segfault in a fairly reproducible way, I did the following on office world:

ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge --use_sim_time
# Wait until the robot is on the way back to its designated charger
ros2 run rmf_demos_tasks dispatch_go_to_place -p tinyRobot2_charger --use_sim_time

This is the log

[fleet_adapter-17] [INFO] [1715866344.935831555] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [19.89569926 -3.40715006  0.39677206] on map [L1]: cmd_id 2
[rviz2-10] 
[rviz2-10] ** (rviz2:2893259): WARNING **: 21:32:28.508: atk-bridge: get_device_events_reply: unknown signature
[fleet_adapter-17] [INFO] [1715866349.558899088] [tinyRobot_fleet_adapter]: Beginning a new parking task for robot [tinyRobot/tinyRobot2]
[fleet_adapter-17] [INFO] [1715866349.559667818] [tinyRobot_fleet_adapter]: Selecting a new go_to_place location from [1] choices for robot [tinyRobot/tinyRobot2]
[fleet_adapter-17] [INFO] [1715866349.559716868] [tinyRobot_fleet_adapter]: Got distance from [28] as 6.920327
[fleet_adapter-17] [ERROR] [1715866349.559834748] [tinyRobot_fleet_adapter]: Unable to find a path to any of the goal options [[place:tinyRobot2_charger]] for [tinyRobot/tinyRobot2]
[fleet_adapter-17] [ERROR] [1715866349.560428308] [tinyRobot_fleet_adapter]: Got ticket issueing claim
[fleet_adapter-17] [ERROR] [1715866349.560583908] [tinyRobot_fleet_adapter]: Claim issued
[queue_manager-14] [INFO] [1715866349.560666328] [rmf_chope_node]: Allocating 28 to 4
[fleet_adapter-17] [INFO] [1715866349.561145268] [tinyRobot_fleet_adapter]: Selecting a new go_to_place location from [1] choices for robot [tinyRobot/tinyRobot2]
[fleet_adapter-17] [INFO] [1715866349.561259908] [tinyRobot_fleet_adapter]: Planning for [tinyRobot/tinyRobot2] to [tinyRobot2_charger] from one of these locations:
[fleet_adapter-17]  -- lane 24: { L1 <  18.729 -3.89598> [patrol_A2] } -> { L1 < 19.8957 -3.40715> [lounge] } | location < 19.8835 -3.41239> | orientation 22.7334
[fleet_adapter-17]  -- lane 25: { L1 < 19.8957 -3.40715> [lounge] } -> { L1 <  18.729 -3.89598> [patrol_A2] } | location < 19.8835 -3.41239> | orientation 22.7334
[fleet_adapter-17] [INFO] [1715866349.564905018] [tinyRobot_fleet_adapter]: Executing go_to_place [tinyRobot2_charger] for robot [tinyRobot/tinyRobot2]
[fleet_adapter-17] [ERROR] [1715866349.566171208] [tinyRobot_fleet_adapter]: Releasing waypoint
[queue_manager-14] [INFO] [1715866349.566286968] [rmf_chope_node]: Releasing ticket for 3
[fleet_adapter-17] [INFO] [1715866349.567795628] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [18.72901862 -3.8959816  -0.6960909 ] on map [L1]: cmd_id 3
[rmf_task_dispatcher-13] [INFO] [1715866352.777068445] [rmf_dispatcher_node]: Add Task [compose.dispatch-1] to a bidding queue
[rmf_task_dispatcher-13] [INFO] [1715866352.913008243] [rmf_dispatcher_node]:  - Start new bidding task: compose.dispatch-1
[fleet_adapter-17] [INFO] [1715866352.913647203] [tinyRobot_fleet_adapter]: [Bidder] Received Bidding notice for task_id [compose.dispatch-1]
[fleet_adapter-17] [INFO] [1715866352.914766214] [tinyRobot_fleet_adapter]: Planning for [2] robot(s) and [1] request(s)
[fleet_adapter-17] [INFO] [1715866352.916380434] [tinyRobot_fleet_adapter]: Submitted BidProposal to accommodate task [compose.dispatch-1] by robot [tinyRobot2] with new cost [34.054840]
[rmf_task_dispatcher-13] [INFO] [1715866355.113066131] [rmf_dispatcher_node]: Determined winning Fleet Adapter: [tinyRobot], from 1 responses
[rmf_task_dispatcher-13] [INFO] [1715866355.113148461] [rmf_dispatcher_node]: Dispatcher Bidding Result: task [compose.dispatch-1] is awarded to fleet adapter [tinyRobot], with expected robot [tinyRobot2].
[fleet_adapter-17] [INFO] [1715866355.113777561] [tinyRobot_fleet_adapter]: Bid for task_id [compose.dispatch-1] awarded to fleet [tinyRobot]. Processing request...
[fleet_adapter-17] [INFO] [1715866355.114370381] [tinyRobot_fleet_adapter]: Canceling go_to_place for robot [tinyRobot/tinyRobot2]
[ign-18] [INFO] [1715866355.125845791] [slotcar_tinyRobot2]: [tinyRobot2] already received request [3] -- continuing as normal
[ERROR] [fleet_adapter-17]: process has died [pid 2893273, exit code -11 [...]

I wonder if the "try to reserve a resource that is already reserved" is a corner case that causes this instability?

[arjo129]

Fixed in 18d2f3e. It was an error in the cancellation behaviour.

[xiyuoh]

Thanks for all the progress, this is looking really great!

Here are some observations from further stress testing:

1. Office world, using the office_reservation_node.launch.xml

At startup, with each robot at the respective chargers, I sent tinyRobot2 to tinyRobot1_charger. This leads tinyRobot2 to go to tinyRobot1_charger, even though it is already occupied. Are waypoints claimed and allocated on launch? I suspect this has something to do with the nothing finishing request for this fleet. This only happens when the first task submitted on launch leads a robot to a waypoint currently occupied by another robot.

2. To test out the reservation node in multi-level scenarios, I tested it in Hotel world.

Tweaks to building map, fleet configs and launch file:
I added a parking spot to an existing waypoint on L3, enabled the reservation node in Hotel world's launch file, and in the fleet configs set use_reservation_node to True for both deliveryRobot and tinyRobot fleets. I also set the finishing request to nothing for both fleets.

The reservation system works well as expected when sending robots from different fleets to the same waypoint on the same map 🎉

There is a particular multi-level scenario I was curious about. With both tinyBot_1 and deliveryBot_1 at L1 at their respective chargers, I followed these steps:

• Submit patrol task for tinyBot_1 to L3_master_suite. The robot begins its way up to L3.
• Submit patrol task for deliveryBot_1 to L3_master_suite. The robot stays put at deliverybot_charger.
• After tinyBot_1 has reached L3_master_suite, submit patrol task for tinyBot_1 to L3_room1.
  Observation: tinyBot_1 moves to L3_room1. At the same time at L1, deliveryBot_1 begins moving towards the lift to travel up to L3, then goes to L3_master_suite.

I assume this is the intended behavior, where if the target waypoint is on another level and is occupied, the robot would go to a parking spot on its current map while waiting for the target waypoint to become vacant.
However, I was thinking it might make more sense for deliveryBot_1 to wait at a parking spot on L3 (if one exists) instead of at L1 where it originally was. That way, if L3_master_suite becomes vacant while deliveryBot_1 is making its way up to the L3 parking spot, the robot would already be halfway to the target waypoint.
With the current behavior, the travel time for each robot to travel to a different map becomes exclusive: tinyBot_1 completes the travel from L1 to L3, waits X minutes at L3_master_suite and leaves, then deliveryBot_1 starts travelling from L1 to L3. As opposed to having both robots start moving from L1 to L3 once they receive their tasks, and their travel times would overlap.
I can imagine a significant amount of waiting time saved if this was a larger building, and each robot was planned to occupy the target waypoint for few minutes to perform some action, while the travel time from charging spots to target waypoints could take anywhere from a quarter to half an hour.

This is more of a what-should-be-the-best-behavior consideration, feel free to share your thoughts!

[arjo129]

Are waypoints claimed and allocated on launch? I suspect this has something to do with the nothing finishing request for this fleet.

Sadly, in the current architecture of RMF this is actually very hard to do. There must be some thing that asks the robot to go to a location to register it. Currently, if a robot starts on a location, unless there is a task assigned to it at start up, the robot will not issue a claim. If you notice in my example for the office world. I disable finishing_request on both. I suspect we need to be explicit about this in the documentation.

There is a particular multi-level scenario I was curious about. With both tinyBot_1 and deliveryBot_1 at L1 at their respective chargers, I followed these steps

This is tricky, as we define GoToPlace as having multiple possible end points. If multiple end points are specified it would be hard to know which parking spot is nearest. Currently, the heuristic used is the nearest parking spot from the start point since there may be more than one end point. Perhaps in the event that there is only one end point I should use a different cost function.

[arjo129]

As discussed offline, I have implemented @xiyuoh's recommendation of waiting near the goal destination if we have only one goal destination.

[xiyuoh]

Thanks Arjo! Tested both single and multi level scenarios with a single goal waypoint, in both cases the robot waiting for the goal to be freed up was able to travel to the parking spot nearest to the goal. Other than the minor typo everything LGTM, happy to approve after that is fixed 😄

[arjo129]

Thanks for the thorough feedback. This wouldn't have been possible without it! I've fixed the typo.

All feedback has been addressed

[mxgrey]

I've done a final pass and I'm very happy with how this feature has turned out.

@arjo129 good job cramming this very important feature into a structure that wasn't originally made for it.

@luca-della-vedova thanks for the very thorough initial review!

@xiyuoh thanks for the rigorous testing and catching so many important edge cases!

This was an excellent team effort all around. I'll merge this within the next 24 hours as long as there are no objections.