Identified New Scenario Where In-Place Rotation Can Be Skipped (#661)

Open-RMF Open-RMF General
1

Posted by @alex-roba:

I’ve identified a scenario where the robot’s in-place rotation can be unintentionally skipped. This issue arises when a fleet adapter receives a new go_to_place command that overrides an existing one. Specifically, if the robot is currently rotating to align itself with a docking station, the new go_to_place command can override the correct in-place rotation, causing the robot to approach the dock at an incorrect angle. Below, I’ll describe the problem and provide supporting log evidence.

Scenario Description
Initial State: The robot (nanos10002) is following a path with multiple waypoints, including an in-place rotation to face the dock correctly.
Correct Rotation Command: The robot is commanded to rotate to an angle of -1.57079633 radians (≈ -90°) at position [2.91248069, -0.87074165] (cmd_id 179), preparing it to dock.
Interruption: Before the rotation completes, a new go_to_place request (pgv_dock_m3) is received, specifying a single waypoint with a slightly different position [2.90602542, -0.86592286] and an incorrect angle of -1.73497393 radians (≈ -99.4°, cmd_id 180).
Result: The robot navigates to this new waypoint, overriding the correct rotation. It then attempts to dock at [2.91248069, -1.67395578, -1.57079633] (cmd_id 181) with an unaligned initial angle, potentially causing docking failure.
This suggests that the new go_to_place command does not preserve the robot’s ongoing rotation state, leading to misalignment.

[fleet_adapter-1] [INFO] [1744083666.468903268] [Robots100loading_fleet_adapter]: All mutex groups were already locked for [Robots100loading/nanos10002]
[fleet_adapter-1] [INFO] [1744083666.469979447] [Robots100loading_command_handle]: Received new follow path request for robot [nanos10002] with 6 waypoints which are:
[fleet_adapter-1] [INFO] [1744083666.470730243] [Robots100loading_command_handle]: Waypoint: wp_7 at [ 5.13437317 -1.621381    1.57079633] on map L1
[fleet_adapter-1] [INFO] [1744083666.471417627] [Robots100loading_command_handle]: Waypoint: wp_3 at [ 5.13437317 -0.87074165  1.57079633] on map L1
[fleet_adapter-1] [INFO] [1744083666.472153379] [Robots100loading_command_handle]: Waypoint: wp_3 at [ 5.13437317 -0.87074165  3.14159265] on map L1
[fleet_adapter-1] [INFO] [1744083666.472816431] [Robots100loading_command_handle]: Waypoint:  at [ 3.86909548 -0.87074165  3.14159265] on map L1
[fleet_adapter-1] [INFO] [1744083666.473528103] [Robots100loading_command_handle]: Waypoint:  at [ 2.91248069 -0.87074165  3.14159265] on map L1
[fleet_adapter-1] [INFO] [1744083666.474228717] [Robots100loading_command_handle]: Waypoint:  at [ 2.91248069 -0.87074165 -1.57079633] on map L1
[fleet_adapter-1] [INFO] [1744083666.474857221] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to wp_7 at [ 5.13437317 -1.621381    1.57079633] on map [L1]: cmd_id 174
[fleet_adapter-1] [INFO] [1744083670.093262726] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to wp_3 at [ 5.13437317 -0.87074165  1.57079633] on map [L1]: cmd_id 175
[fleet_adapter-1] [INFO] [1744083675.730674145] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to wp_3 at [ 5.13437317 -0.87074165  3.14159265] on map [L1]: cmd_id 176
[fleet_adapter-1] [INFO] [1744083679.252714801] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to  at [ 3.86909548 -0.87074165  3.14159265] on map [L1]: cmd_id 177
[fleet_adapter-1] [INFO] [1744083683.881142164] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to  at [ 2.91248069 -0.87074165  3.14159265] on map [L1]: cmd_id 178
[fleet_adapter-1] [INFO] [1744083688.219801130] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to  at [ 2.91248069 -0.87074165 -1.57079633] on map [L1]: cmd_id 179		----- correct angle 
[fleet_adapter-1] [INFO] [1744083690.965974420] [Robots100loading_fleet_adapter]: Executing final go_to_place [pgv_dock_m3] for robot [Robots100loading/nanos10002]
[fleet_adapter-1] [INFO] [1744083690.967562216] [Robots100loading_command_handle]: Received new follow path request for robot [nanos10002] with 1 waypoints which are:
[fleet_adapter-1] [INFO] [1744083690.968520536] [Robots100loading_command_handle]: Waypoint:  at [ 2.90602542 -0.86592286 -1.73497393] on map L1
[fleet_adapter-1] [INFO] [1744083692.075082287] [Robots100loading_command_handle]: Commanding [nanos10002] to navigate to  at [ 2.90602542 -0.86592286 -1.73497393] on map [L1]: cmd_id 180		------ wrong angle 
[fleet_adapter-1] [INFO] [1744083692.843438326] [Robots100loading_command_handle]: Commanding [nanos10002] to dock to pgv_dock_m3 at [ 2.91248069 -1.67395578 -1.57079633] on map [L1]: cmd_id 181
2

Posted by @xiyuoh:

Hello @alex-roba , there was a previous issue [Bug]: skip_rotation_commands: False Still Results in Consecutive Commands with Misaligned Angles · Issue #556 · open-rmf/rmf · GitHub regarding in-place rotation. Before investigating this scenario more in-depth, can I check that your setup includes changes from these PRs: Insert in-place rotation waypoint if missing by xiyuoh · Pull Request #111 · open-rmf/rmf_traffic · GitHub and Do not skip waypoint if orientation is not aligned by xiyuoh · Pull Request #398 · open-rmf/rmf_ros2 · GitHub.

If this still happens with the abovementioned fixes, please provide more details about your setup (i.e. source or binaries, which RMF version, etc.). I might also need a better idea of the flow of events, as it’s hard to understand where the waypoints are and why it is trying to go to a new waypoint pgv_dock_m3 between docking attempts. If possible please provide some diagrams, nav graph, or videos/screenshots of your RViz when this is happening - it’ll help me reproduce the issue and find a solution for this particular scenario.

3

Posted by @alex-roba:

@xiyuoh Yes, I’m using the code from the main repository, which includes changes from both PRs. The yellow region represents a mutex. Regarding your question about why the robot is trying to navigate to a new waypoint (pgv_dock_m3): it’s not actually a new waypoint—it’s the same original waypoint (pgv_dock_m3). However, I’m unsure why the fleet adapter received a new “Executing final go_to_place” command for the same waypoint, especially since the previous plan was fine and I didn’t request replanning.

image
image600×424 122 KB

Edited by @alex-roba at 2025-04-08T08:28:45Z

4

Posted by @alex-roba:

@xiyuoh I found out why a new “Executing final go_to_place” command is issued for the same waypoint. It happens because there was a negotiation occurring at that moment for the robot.

The specific scenario is this: when a negotiation takes place while the robot is at the waypoint just before the dock, and it’s rotating to align with the dock, the negotiation results in a new command being received — but with an incorrect angle. Once the robot finishes executing that command, the fleet adapter proceeds to issue a dock command without considering the angular continuity or proper alignment.

Edited by @alex-roba at 2025-04-09T05:01:45Z

5

Posted by @xiyuoh:

Hi @alex-roba , while I wasn’t able to time my simulation to reproduce what you’re experiencing, I opened a PR Ensure correct docking orientation by xiyuoh · Pull Request #414 · open-rmf/rmf_ros2 · GitHub to ensure correct robot orientation before docking. Can you test it out and let me know if it works for you?

6

Posted by @xiyuoh:

I’ll close this as the behavior should be fixed by Ensure correct docking orientation by xiyuoh · Pull Request #414 · open-rmf/rmf_ros2 · GitHub, but if the issue persists feel free to reopen the ticket.