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.