I found a scenario where the lane orientation is not being respected — it happens during docking. I was able to reproduce the issue in the office world environment with main . I’ll share the building file and configuration for reference.
To reproduce it, you just need to dispatch the robot to the lounge using the “Go to Place” command. As you’ll see, I created a new waypoint between lounge and patrol_a, and modified the orientation of the two connecting lanes to be backward. And make lounge docking waypoint.
However, after reaching the middle waypoint, the robot still rotates to face forward. This happens because the fleet adapter assigns a destination orientation of 0 instead of 180, causing the robot to approach the next lane forward rather than respecting the intended backward direction. This breaks the expected docking alignment.
Are you confident that rmf_fleet_adapter is sending the wrong orientation? I think the slotcar plugin in the simulator might be set up to just drive towards a goal forwards or backwards based on whichever requires less rotation, so this might be a case of disobedience rather than rmf_fleet_adapter asking for the wrong orientation.
Hi @mxgrey, yes, I’m sure about this — I observed the same behavior on my physical robot. Interestingly, the issue disappears when I change the lounge waypoint to be a non-docking waypoint.
Here’s a snippet from the logs that highlights the problem:
[fleet_adapter-16] [INFO] [1744186338.048690381] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [18.94976486 -4.03908233 3.14159265] on map [L1]: cmd_id 29
# This is patrol_a – robot rotates to face backward (180°) as expected.
[fleet_adapter-16] [INFO] [1744186342.138263955] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [20.18761492 -4.03908233 3.14159265] on map [L1]: cmd_id 30
# This is the middle waypoint – orientation is 180°, correct.
[fleet_adapter-16] [INFO] [1744186350.838310752] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [21.01599746 -4.03908233 0. ] on map [L1]: cmd_id 31
# When commanded to go to the lounge, the angle is 0° instead of 180°, which breaks the intended backward lane orientation.
I see, the purpose of marking of a point as a docking location is because you’re expected to have a separate routine that you run for docking.
In the original API you would actually just receive the docking name with no position information at all. The idea was that the unique name of the dock should map to a specific routine in your adapter, and that routine should know where the robot is supposed to go and how it should go there.
In practice we found that many users still wanted some coordinates to refer to, not just the unique docking name. So for the Easy Full Control API we re-introduced the position coordinates. However we look up the coordinates in the nav graph which doesn’t give any indication of orientation, so we might just be filling in the orientation with 0.
If you want to rely on the coordinates received from the command to get the correct orientation, I would recommend not making this a docking point. If you do want special docking behavior then I recommend encoding the behavior somehow, either in your adapter or in the dock name. Since the “dock name” can actually be any string, in some deployments we’ve put an entire json structure in where the json contains parameters that describe how the docking should be performed.
For the next generation we’ll be replacing this clunky “dock name” concept with the ability to specify a service/action to run when the robot reaches this point, and allow you to specify arguments to pass to the service/action.
@mxgrey I used to rely solely on the docking waypoint name to initiate docking. However, I noticed that when a negotiation occurs while the robot is at the waypoint just before the dock — particularly during its rotation to align with the dock — the negotiation results in a new navigation command, but with an incorrect angle.
After completing that command, the fleet adapter proceeds with the docking request. To handle this, I decided to use the angle from the dock command to send a pre-alignment goal to the robot before triggering the actual docking behavior.
However, as shown in the logs, that angle can’t always be trusted — in some cases, it’s incorrect, which causes misalignment during docking.
My backlog is too long at the moment to resolve this, but if you want to experiment with fixing it then here is where the vertices are analyzed and here is where the angle is calculated. As you can see we’re just doing a very basic atan2.
You could check the orientation constraint on the found lane and then flip the orientation if it’s supposed to be backwards. Feel free to open a PR for that.