Random u-turns with RMF

[keshavchintamani]

Hi All,

We are testing RMF with a tugging robot in a production environment. The goal of the system is to bring the cart to a goal waypoint and NOT U-TURN at any time. However very randomly RMF starts making the robot U-TURN. frequently in the same region never anywhere else in the nav graph

Here’s the scenario:

• NavigateThroughPoses actions is executing, robot is travelling and arriving close to its goal (Yellow Path in Image)
• Randomly, the robot swings around when it receives an RMF new path request (Red path pointing to the new goal)
• The new path request seems to be asking the robot to come back to a previous waypoint.

RMF makes the robot swing around even on one way lanes.

We set reversible to False

What lowered the frequency of occurences to the some degree were

1. Disabling the delay_threshold

delay_threshold:=100000.0 disable_delay_threshold:=true

2. Reducing the velocities and accelerations to lower than the robot’s nominal values

linear_velocity:= angular_velocity:= linear_acceleration:=angular_acceleration:=0.2

3. Increasing the state topic rates

update_state_frequency:=20.0 publish_state_frequency:=2.0

Our Configuration

Packages: ros-humble-rmf-*
Version: 3.0.3-1jammy.20250718.234906

Fleet adapter is custom but using the older C++ version with free_fleet/free_server using Zenoh DDS bridge.

Any ideas on if there is API or class which can disable this behavior is really welcome!!

[grey]

Randomly, the robot swings around when it receives an RMF new path request (Red path pointing to the new goal)

Just out of curiosity, is the new path request triggered because it’s moving on to the next phase of its task, or is something else triggering it?

The new path request seems to be asking the robot to come back to a previous waypoint.

The reasons for “random swinging” and backtracking at the start of a new path are complex, but mostly boil down to this: The async / distributed relationship between the fleet adapter and the AMR means there’s inherently some latency and disconnect between the information that the fleet adapter is using to make decisions and the ground truth that the robot is experiencing.

The first waypoint in a new path command serves a special purpose: It indicates the point in space that the fleet adapter intended the robot to start this new path from. But it’s possible that your robot is no longer in the same location that it was when the plan was initially generated. The general recommendation, especially in a case like yours where backtracking has serious implications, is to filter out waypoints within the path that your robot has already passed by, somewhat similar to a Pure Pursuit controller. This will require a custom fleet adapter that responds directly to the follow_new_path interface.

RMF makes the robot swing around even on one way lanes.

In general only the first waypoint in the path should ever send you in the “wrong” direction on a one-way lane, because of the temporal inconsistency that I mentioned above (or maybe the first N waypoints if your robot is doing a lot of extra motion and there is a significant delay in the motion planner).

If you detect a particularly egregious path that your robot is unable to follow, I recommend following these steps:

1. Make sure the robot is fully stopped and then update the robot’s location as precisely as possible using one of the RobotUpdateHandle methods.
2. Call the RobotUpdateHandle::replan to ask for a new plan from this precise stopped position.

That should be enough to get a reasonable plan, but you will still need to filter out at least the first waypoint.

We set reversible to False

In the fleet adapter settings “reversible” determines whether or not the robot can drive backwards. I.e. drive opposite of the direction that it’s facing. This setting does not relate to the ability of a robot to stop, turn around, and drive back in the direction that it had been coming from.

The most important thing for a tugging robot will be to make sure that every single lane is very rigorously set to be one-directional, and you filter out any waypoints in the commanded path that would cause you to backtrack.

Truthfully we were not accounting for tugging robots in the original implementation of Open-RMF, so the planner doesn’t really have the right settings to rigorously constraint the motion to fit your use case. I’ll be interested to hear about your experience with integrating your robot, and we can look into supporting the right kinds of constraints in the next generation traffic planning that we’re working on.

[keshavchintamani]

Hi @grey

@grey

Just out of curiosity, is the new path request triggered because it’s moving on to the next phase of its task, or is something else triggering it?

See image for the actual scenario. I’ve designed one-directional lanes carefully to avoid reversals but cant everywhere because of the environment. This issue happens on one-directional lanes, which is bad.

RMF always gives the exact same "green path"to Nav2. This path is a 80 meter long path with many WPs. The new “u-turn” path is always much shorter and always in the same region. Typically a few waypoints behind the robot’s current position. Its unclear if or when RMF (or the fleet adapter) decides on the new path :

1. before arrival at the current path’s goal WP

2. or after current phase has been completed

Not easy to catch in a live environment - RMF→nav2 coordinate transforms + fleet_adapter→ fleet_server→ fleet_client async messaging…. all dockerized.

Could you please clarify what impact the vehicle velocity/acceleration params have on the fleet_adapter? In the RMF rviz view, you can see the estimator “following” or “trailing” the robot.

Is it possible that the travel estimator is way slower than the robot and forces a replan inside RMF traffic. AFAIK we dont check this in our fleet_adapter…

@grey

The first waypoint in a new path command serves a special purpose: It indicates the point in space that the fleet adapter intended the robot to start this new path from. But it’s possible that your robot is no longer in the same location that it was when the plan was initially generated. The general recommendation, especially in a case like yours where backtracking has serious implications, is to filter out waypoints within the path that your robot has already passed by, somewhat similar to a Pure Pursuit controller. This will require a custom fleet adapter that responds directly to the follow_new_path interface.

Isnt RMF already filtering out the waypoints that have already passed when it sends a new path? Unless the fleet_adapter are not getting fleet_state messages fast enough.

fleet client → [robot_state @ 20hz] → fleet_server → [fleet_states @ 10Hz] → Fleet_adapter

Should be plenty fast unless there are major network dropouts which is not the case.

@grey

• Make sure the robot is fully stopped and then update the robot’s location as precisely as possible using one of the RobotUpdateHandle methods

AFAIK, there is insufficient context to do this in the fleet adapter. Only possible location seems in the fleet_client, also to filter WPs like you suggested

@grey

The most important thing for a tugging robot will be to make sure that every single lane is very rigorously set to be one-directional,

I’d be very happy to share experience. There are a lot of learnings not just for tuggers but this issue is really sticky and unfortunately urgent. Any help is appreciated.

[grey]

Is it possible that the travel estimator is way slower than the robot and forces a replan inside RMF traffic.

If the new path is being triggered by a replan request, the standard log will indicate that a replan was requested and it will also specify the reason. If you can provide the log entries that mention the replan, then I should be able to give you an exact reason that it’s happening.

RMF always gives the exact same "green path"to Nav2. This path is a 80 meter long path with many WPs. The new “u-turn” path is always much shorter and always in the same region. Typically a few waypoints behind the robot’s current position.

I’m not totally sure if I’m following your description and the illustration correctly. Are you saying that the new “follow path request” coming from Open-RMF is the red line, outlined in a blue below, and that the newly request path starts 5 waypoints behind the robot’s actual location, like the blue arrows that I’ve drawn on top of the red path below?

If that’s what you’re observing then somewhere in the pipeline there’s a significant error with how the robot’s location is being updated.

Fleet adapter is custom

I’ll need to know more about what kind of customization you’ve done and which specific APIs you’re using. In particular are you using the RobotCommandHandle API or the EasyFullControl API? If you’re primarily using humble packages then EasyFullControl wasn’t available when those versions were released, but the current main branch of rmf_ros2 is still compatible with ROS humble, so you could still use EasyFullControl if you build the main branch from the source code.

Isnt RMF already filtering out the waypoints that have already passed when it sends a new path?

Not necessarily.

• If you use the RobotCommandHandle API then nothing is filtered: You get the whole path computed by the planner which will start from whatever location the planner believed the robot was at when it started to compute the plan.
• If you use EasyFullControl, then it will do some filtering to weed out parts of the path that the robot has already passed. It’s a little bit like pure pursuit, but not exactly the same. This usually helps cuts down on extraneous motions for regular AMRs, but it was not made with the constraints of a tugging robot in mind.

AFAIK, there is insufficient context to do this in the fleet adapter. Only possible location seems in the fleet_client, also to filter WPs like you suggested

Just to make sure we’re on the same page, when I talk about the “fleet adapter” I’m generally talking about the part of your system integration code that directly interacts with the rmf_fleet_adapter library. In a case like yours where it’s extremely important that the robot NEVER backtracks, I would probably put the following state machine into my fleet adapter code:

Essentially each time a new follow_new_path comes in, you should check if your robot is currently stopped. If it’s not then ignore the latest plan, ask your robot to stop, after it’s stopped update the position, and then use replan. This will ensure that the path planned for your robot is always beginning from the most accurate initial location.

To be very blunt, the current implementation of Open-RMF’s traffic planning system has never taken into consideration robots that are critically unable to turn around and go backwards. Integrating that kind of robot with Open-RMF is a bit like trying to fit a circular peg in a square hole. The traffic planner is ignorant about this constraint that you have, and there’s a significant risk that it will produce plans which are not feasible for your robot.

This risk is especially significant if you have any bi-directional lanes. If a robot’s task gets cancelled while it’s tugging a payload down a bi-directional lane, there’s a risk that Open-RMF will decide the robot needs to immediately turn around and go back in the direction that it had been coming from, e.g. to park or to charge. You’ll need to make sure that your robots can safely do a u-turn in any area where you’ve put a bi-directional lane.

With enough workarounds, the current traffic system could probably work well enough for a pilot project that involves tugging payloads, but I wouldn’t recommend putting it into a business-critical deployment without a considerable amount of testing and evaluation first. This is not use case that we’ve targeted before, so I can’t offer much assurance about how effective it will be.