[Bug]: More Scenario where Premature Task Completion when using Parking Reservation (use_parking_reservation: true) can happen

[alex-roba]

Before proceeding, is there an existing issue or discussion for this?

• I have done a search for similar issues and discussions.

OS and version

Ubuntu 24.04

Open-RMF installation type

Source build

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

main

ROS distribution

Jazzy

ROS installation type

Binaries

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

There are two related logic bugs in the GoToPlace event (specifically in rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp) that occur when a robot is assigned a Waitpoint (e.g., a parking spot via the reservation system) but the planner calculates an empty plan because the robot is already at that location.

False Completion: The task status is incorrectly marked as Completed instead of Standby when max_merge_waypoint_distance is > default value hence, the robot receive empty path when waiting at the parking spot.

Unresponsive Watchdog Loop: The previous movement command is not explicitly stopped. This leaves the MoveRobot phase active in the background. Since the robot is stationary at the waitpoint, no feedback updates are sent. This triggers the 10-second watchdog timer, causing an infinite loop of "Unresponsive command handle" warnings and unnecessary replanning.

Steps to reproduce the bug

1. Configuration: Set the max_merge_waypoint_distance parameter for the fleet adapter to a value large enough to trigger an empty plan when the robot is near a waypoint (e.g., 0.09 meters).
   Note: If this value is too small (default), the planner typically generates a tiny path adjustment, masking the bug. Setting it to 0.09 ensures plan.get_waypoints() returns empty when the robot is at the parking spot.
2. Enable the Parking Spot Manager/Reservation system.
3. Occupy the robot's final destination with another robot.
4. Issue a GoToPlace task to the occupied final destination.

Expected behavior

No response

Actual behavior

No response

Additional information or screenshots

No response

[xiyuoh]

Hey @alex-roba , I managed to reproduce what you're seeing by making the following changes to the current Office world demo:

• Set max_merge_waypoint_distance to 0.09 (as suggested in this ticket)
• Set is_parking_spot to False for supplies and tinyRobot2_charger so that the planner won't just send the robot to another parking spot since the current one is taken.
• Set finishing request for both robots to nothing, so that the robots would stay at their destinations (making sure that the destination stays occupied)

@mxgrey and I were discussing this ticket and came up with this solution, do you mind trying out this PR and check if the bug still persists for you? After testing it on my end the task status correctly reflects standby if it is sitting on a parking spot, and when tinyRobot2 moves out of the destination, tinyRobot1 is able to resume its task and travel to the destination waypoint.

For subsequent tasks (same run with follow-up patrol tasks), I encountered another error where the robot would be stuck at its current location (a parking spot it is occupying) because the reservation node can't find an empty parking spot for it to wait. It doesn't seem to recognize that it is already sitting on one. Logs:

[fleet_adapter-17] [INFO] [1767172757.522459029] [tinyRobot_fleet_adapter]: Direct request [patrol_8603e804-07c2-4797-a1e7-71397a832667] successfully queued for robot [tinyRobot1]
[fleet_adapter-17] [INFO] [1767172757.522647449] [tinyRobot_fleet_adapter]: Beginning next task [patrol_8603e804-07c2-4797-a1e7-71397a832667] for robot [tinyRobot/tinyRobot1]
[fleet_adapter-17] [INFO] [1767172757.522856839] [tinyRobot_fleet_adapter]: tinyRobot/tinyRobot1: Sending reservation request
[fleet_adapter-17] [INFO] [1767172757.522897179] [tinyRobot_fleet_adapter]: Beginning new task [patrol_8603e804-07c2-4797-a1e7-71397a832667] for [tinyRobot/tinyRobot1]. Remaining queue size: 0
[fleet_adapter-17] [INFO] [1767172757.522920779] [tinyRobot_fleet_adapter]: Reservation Negotiator: Selecting a new go_to_place location from [1] choices for robot [tinyRobot/tinyRobot1]
[fleet_adapter-17] [INFO] [1767172757.522937639] [tinyRobot_fleet_adapter]: Got distance from [15] as 29.274374
[fleet_adapter-17] [INFO] [1767172757.523790229] [tinyRobot_fleet_adapter]: Sorting waitpoint by distance from goal 15
[queue_manager-14] [ERROR] [1767172757.524073469] [rmf_reservation_node]: Could not find a free space from any of: lounge, 
[queue_manager-14] [INFO] [1767172757.524159789] [rmf_reservation_node]: Could not immediately service 11 enqueing. Locations in ticket were:
[queue_manager-14] 	- lounge
[queue_manager-14] 
[queue_manager-14] [ERROR] [1767172757.524190419] [rmf_reservation_node]: Could not find a free space from any of: tinyRobot1_charger, 
[queue_manager-14] [ERROR] [1767172757.524219769] [rmf_reservation_node]: Could not allocate a waiting point for robots from 1 waitpoints

This is also fixed this in the PR linked above.

[alex-roba]

@xiyuoh the issue is solved with the PR

[xiyuoh]

@alex-roba Awesome, thanks for testing and checking! I'm still adding other fixes to the PR but it shouldn't affect this particular behavior. Will close this ticket after the PR is merged.