Robots stop on every waypoint on a straight line

[cwrx777]

Hi,

rmf release: iron_20241229

I have created a map as shown below, to simulate robot moving through a long corridor with some waypoints for queueing, (with y-value of each waypoint on the long straight line being the same).

waypoint-straight-line-2024-04-19_13-32-26.mp4

at 0:43, a task was dispatched to DeliveryRobot_02.
I hope it can be seen from the video that DeliveryRobot_02 stopped at every waypoint to reach the destination, while DeliveryRobot_01 did not.

The path request to DeliveryRobot_01 was to move to pos_9, pos_11, pos_19, and OR_3.
For DeliveryRobot_02. it was from pos_01 to pos_02, followed by another path request from pos_02 to pos_03, and so on.

[mxgrey]

The second robot's planner is aware of the first robot's plan, but doesn't necessarily know how much progress the first robot has made. So the safest thing to do for the second robot is to stop at each waypoint that overlaps with the first robot's path and check if the first robot has made it past the second robot's next waypoint before moving forward. That process will get repeated over and over.

RMF used to work the way you'd prefer, where we didn't consider a "traffic conflict" to exist if two robots are moving in the same direction. But then in practice we found that the robot that's behind would often attempt to overtake the robot that's in front whenever the robot in front gets delayed by something, e.g. an obstacle. The attempt to overtake the robot in front would easily lead to really bad traffic jams and then all the robots get stuck. So the current behavior is non-optimal, especially in long stretches like this, but safe and reliable, which is usually the more important factor with robot operations.

I haven't addressed this yet because the way task executions are implemented in the current generation of RMF is very rigid and fragile (the fragility is because C++ is very prone to data races, segfaults, and undefined behavior, so any significant code change is a liability that could introduce one of those bugs). By contrast, getting the right behavior here will be very straightforward in the next generation of RMF which we are actively working on, and where I need to focus my time and energy.

I can think of a hacky solution that might be able to smooth this out, but I wouldn't have time to test it as thoroughly as it needs to be tested before I would risk merging it in. If this problem is important enough for your use case that you're willing to extensively test the potential fix, let me know and I can create a branch to work on it.