How to avoid robots collision and deadlock?

[ghost]

Hi, @mxgrey

Recently, I met some very tricky issues when using RMF.

I wrote a new fleet_adapter based on rmf_demos_fleet_adapter.
The main difference between rmf_demos_fleet_adapter and my_fleet_adapter is :
In rmf_demos_fleet_adapter, the waypoints in follow_new_path will be delivered to the robot one by one.
But I need to deliver all the waypoints in the follow_new_path to the robot one time. That I will monitor the robot's location and call update_handle.update_current_waypoint, or update_handle.update_current_lanes to inform RMF in time. At the same time, I will call next_arrival_estimator to update the duration to the next arrival periodically.

With my_fleet_adapter, two robots collision and dead_lock happened more often than with rmf_demos_fleet_adapter. I think it must be related with inaccurate data I fed to RMF core.

I also notice there are some configuration item, such as vicinity, velocity, max_delay and so on, they must be related with traffic planning. But I am not sure how to configure them correctly. Could you please help shed some light on them here? Or anything else you may advise to avoid collision and deadlock?

If I update the next_arrival_estimator to one waypoint as 5 seconds, but in fact the robot arrived at that waypoint after 3 seconds, will this cause two robots collision when the two scheduled paths for the two robots are overlapped at that waypoint?

Thank you very much
Stella

[ghost]

And I have another question about approach_lanes of waypoint in the follow_new_path.
In most cases, approach_lanes are provided for each waypoint, which makes us easy to find the current lane where the robot is moving on. But sometimes, approach_lanes is empty.
In which kind of case empty approach_lanes will be provided for one waypoint?
In this case, how do we determine the current_lane for the robot?
If I cannot get current lane correctly, how can I inform RMF core about it?

[mxgrey]

In which kind of case empty approach_lanes will be provided for one waypoint?

This means the planner believes the robot is starting from a point that's not on the nav graph. It believes the robot's location is so far from any lane that it doesn't make sense to say that the robot is on any lane at all. So the planner is commanding the robot to go to whichever waypoint is nearby to get the robot back onto the navigation graph. After it arrives at that waypoint, you should start to see approach lanes again for the next waypoint commands.

how do we determine the current_lane for the robot?

You could use this function to let RMF attempt to find the right information for your robot, but there's some risk to it: This function is not aware of walls that exist in your environment, so it might try to have your robot move through a wall if the closest nav graph element is on the other side of that wall. Depending on your application, this might or might not be an issue. The next generation of RMF will have a built in way of handling this correctly, but for now you'd need to implement your own solution in your fleet adapter if this problem is a risk for you.

If I cannot get current lane correctly, how can I inform RMF core about it?

Assuming you're using the Python API, you'll want to use this function to tell the planner where the robot is and which waypoint it should approach to get back onto the navigation graph. The equivalent C++ function is here.

[ghost]

Can I avoid collision by enlarging the vicinity to 3.0m?

[mxgrey]

Enlarging the vicinity is not likely to fix the issue you're experiencing. It's more likely that a larger vicinity will constrain the traffic so badly that traffic negotiations will fail entirely.

[mxgrey]

If I update the next_arrival_estimator to one waypoint as 5 seconds, but in fact the robot arrived at that waypoint after 3 seconds, will this cause two robots collision when the two scheduled paths for the two robots are overlapped at that waypoint?

I might need more information about what exactly you're experiencing, but in theory the accuracy of the arrival estimation should not be a significant issue. If two robots have a conflict, then one is supposed to wait on the other until it is truly out of the way. The traffic dependency system should not allow a slightly inaccurate estimation to trigger a robot to advance prematurely.

Therefore I suspect there is a different problem happening in your scenarios. Can you provide us with a simple .building.yaml and scenario (e.g. Robot A goes from Waypoint X to Waypoint Y while Robot B goes from Waypoint U to Waypoint V) that triggers the bad behavior you're describing?

[ghost]

And in the following scenario, deadlock will happen between robotA and robotB
test scenario:

1. robotA is on P13, robotB is on P14
2. request robotA to P14, at the same time request robotB to P13

Then robotA and robotB will enter deadlock status.
What I expect is:

1. robotA moves to somewhere, for example P10
2. robotB moves to somewhere, for example P17
   then
3. robotA goes to P14
4. robotB goes to P13

[mxgrey]

Then, robotA and roboB collided at P17.

Are you able to reproduce this collision in an rmf_demos simulation? I have two separate thoughts on the matter:

1. The traffic dependency system should typically prevent Robot A and Robot B from approaching P17 at the same time, so the situation you described isn't supposed to happen if arrival updates are being given to the fleet adapter correctly. But if there's a flaw in the arrival update information (such as passing the wrong path_index into next_arrival_estimator or calling the path_finished_callback before the robot has arrived at its final waypoint) then the traffic system might think that the conflict has passed earlier than it really has.
2. RMF is currently designed to prevent traffic conflicts, not physical collisions. Ultimately it is the responsibility of a robot's on-board sensors to make sure it does not physically collide with anything else. RMF's role is to eliminate conflicts where robots would be deadlocked. In the next generation of RMF we'll introduce an option for more conservative traffic behaviors, but hardware-level safety will still be mandatory because even a very conservative traffic system can't guarantee safety when there may be issues with network loss or localization errors.

Then robotA and robotB will enter deadlock status.

The scenario you're describing is not well supported by the current implementation of RMF's traffic system. One problem in particular is that the traffic system will not always behave in a desirable way when robots are left idle on waypoints that obstruct traffic flow or when their goal waypoint would obstruct the traffic flow of other robots. Since P13 and P14 are both waypoints that can interfere with traffic, there is a risk that the traffic negotiation system will fail to come up with a correct plan for them. To help you understand why, the traffic system only "looks ahead" up to the point where a robot has reached its goal. If RobotA wants to go from P13->P14 and RobotB wants to go from P14->P13 then each robot might think that it can wait in place until the other robot reaches its goal and then vanishes from the schedule. Obviously neither robot will actually vanish, but the schedule doesn't realize that.

Is this test a realistic use case for the robot application that you're trying to develop? The current traffic system implementation targets delivery applications where the pick-up and drop-off locations are outside the flow of traffic. If possible I would recommend narrowing the scope of your testing to that use case, assuming that's sufficient for your application.

If these tests do reflect your actual needs, then we're working to add reliable support for this kind of behavior, but it won't be ready in the immediate future because we need to develop several capabilities including the reservation system and free space planning before we can make this use case robust.

[ghost]

Hi, Grey

When you say " The current traffic system implementation targets delivery applications where the pick-up and drop-off locations are outside the flow of traffic", do you mean the pick-up and drop-off locations must be leaf node, right?

[mxgrey]

do you mean the pick-up and drop-off locations must be leaf node

That's right. But it's not sufficient for them to be leaf nodes. They also should be far enough from a lane that the physical robot will not be an obstruction for traffic.

And similar to pick ups and drop offs, any go_to_place tasks are only reliable if the goals are leaf nodes that do not interfere with traffic. You can try to issue them and sometimes or even many times it might work okay, but there are known issues with those use cases that we're actively working to resolve.

[ghost]

1. But if there's a flaw in the arrival update information (such as passing the wrong path_index into next_arrival_estimator or calling the path_finished_callback before the robot has arrived at its final waypoint) then the traffic system might think that the conflict has passed earlier than it really has.

I want to double confirm that the path_index is the index in the list of waypoints received in the follow_new_path, right?
for example,