Prevent RMF from rotating the AMR to face the next waypoint.

[cwrx777]

Hi,
I observe that the waypoints in the path request will make the AMR face the next waypoint.
Consider the traffic-editor map below.

When the AMR needs to travel from pos_A to pos_E, RMF will make the robot stop at pos_B, and rotate to face pos_C, stop at pos_D, and rotate to face pos_E.
Is it possible for RMF not to do the rotation such that the AMR is facing south at pos_B and facing east at pos_D? And of course, when the AMR is traveling from pos_E to pos_A (return trip), the pose at pos_C is to face west.

This is important for the following consideration:

1. when the wifi is disconnected on lane from pos_A to pos_B, the AMR will wait at pos_A until it is connected and receives the next command from the fleet adapter. While waiting at pos_A, we don't want the AMR at pos_B to face pos_C as this will block the corridor.
2. if the AMR needs to rotate at the pos_A, that means pos_A waypoint must be placed a little further from the wall to allow AMR to rotate. but sometimes the facilities need the AMR to keep left. and if the AMR need to be put further away from the wall, when it is facing east/south-east at pos_B, it will block the corridor.
3. Why not combine pos_B and pos_C as one waypoint at the T-junction? we don't want the AMR to wait there blocking human traffic while waiting for wifi to recover.
4. in the case of deconfliction, we also don't want the AMR from blocking human traffic.

[mxgrey]

I'd suggest simply designing your fleet adapter to ignore the rotation command. When you issue commands to your AMR, just don't put in an orientation goal if the AMR's API allows you to exclude that. You can also skip over commanded path waypoints that are on the same vertex and only differ by the orientation.

RMF should be robust to skipping over waypoints in the path command. Just make sure to use the correct path index when you trigger the next_arrival_estimator callback.

[cwrx777]

Hi @mxgrey ,

Consider the following path request: wp[path index]:[waypoint position (x,y,yaw)]-[waypoint time (HH:mm:ss)]:
wp[0]:[0, 0, 45]-[00:00:30]
wp[1]:[0, 1, 45]-[00:00:40]
wp[2]:[0, 1, 60]-[00:00:59]

The robot starting position is at 0,0,0.

1. First iteration:

• set target_pose to wp[0].
• since the robot is already at target_pose but with a different orientation, the fleet adapter will skip this waypoint.
• Question:
  • does the fleet adapter need to wait from the current time until [00:00:30] before calling next_arrival_estimator(0, duration(seconds = 0)) and continue to the second iteration?

2. Second iteration.

• set target_pose to wp[1]
• fleet adapter commands the robot to move to wp[1] and continue to the next iteration.

3. Third iteration

• set target_pose to wp[2]
• since the robot is already at target_pose but with a different orientation, the fleet adapter will skip this waypoint and indicate the path request as finished.
• Question:
  • does the fleet adapter need to wait from the current time until [00:00:59] before calling path_finished_callback?

[mxgrey]

does the fleet adapter need to wait from the current time until ...

Nope, there's no need to wait in any scenario if you're using the 22.09 release of RMF or something more recent. The pertinent PR is this one. From that PR onwards, it is fine for you to completely ignore the time value of each waypoint. Just trigger the callbacks according to whatever your robot is doing, and they should work fine. The only things I would caution against are:

1. Calling next_arrival_estimator(i+1, t) before your robot has arrived at the (x, y) coordinates of waypoint i (it's okay to ignore the orientation and time values of i)
2. Calling next_arrival_callback(i-1, t) after you've already called next_arrival_callback(i, t). I'm not sure specifically what issues this could cause but I imagine it could confuse the system.
3. Calling path_finished_callback before your robot has arrived at the (x, y) coordinates of the last waypoint. Again, it's okay to ignore the orientation and time values of the last waypoint.

[cwrx777]

Hi @mxgrey

Thanks for your reply.

How does RMF know that the robot has reached a waypoint?
Is it correct for the fleet adapter to provide a very low (or 0.0) t value at the beginning of the travel (as the robot started to move), then high t value (based on remaining distance and current robot velocity), and gradually to a small/0 t value again.

[mxgrey]

How does RMF know that the robot has reached a waypoint?

RMF makes an inference that when you say next_arrival_estimator(i+1, t) then your robot has arrived at waypoint i and is moving past it.

Is it correct for the fleet adapter to provide a very low (or 0.0) t value at the beginning of the travel (as the robot started to move), then high t value (based on remaining distance and current robot velocity), and gradually to a small/0 t value again.

This would be a bad idea because it would make the delay management system in the traffic schedule unstable. Saying next_arrival_estimator(i, 0.0) when you're far from waypoint i will make the traffic schedule think your robot is way ahead of schedule from where it really is. That won't impact the traffic dependency system which decides when it's safe for a waiting robot to continue, but it does effect the delay-based replanning system. When one robot is waiting on another robot and it detects a prolonged delay then it might attempt to replan its itinerary. Reporting wildly inconsistent timing estimates will probably cause unnecessary or undesirable replanning behaviors.

For the initial arrival time estimate I recommend simply using distance_between_waypoints / velocity.

In the next generation of RMF we'll be getting rid of this need to estimate your robot's arrival time, but for now it's best to give a reasonable rough estimate for the timing.

[cwrx777]

Hi @mxgrey ,

Assuming the robot has an API to return its current velocity, is it better if the remaining duration is calculated using the robot current velocity?
While the robot is traveling, robot will dynamically adjust its velocity (e.g. due to dynamic object, environment, etc) and the calculated remaining duration may go up (which can be very high if the velocity is very low) and down according to the velocity.

[mxgrey]

The most reliable thing to do is use the velocity value that you believe will be its average speed while moving, or the nominal speed that it aims to use while it's moving.

If you use the true current velocity then that will often drop to zero if the robot detects an obstacle or needs to turn. Calculating the arrival time with a velocity of zero implies the arrival time will be infinite, i.e. the robot can never arrive if it drives at zero velocity.

You don't need to worry about explicitly dealing with the fact that the robot might slow down or speed up, because the traffic schedule will simply shift its expectations every time you report a new arrival estimate. It's best to give rough estimates for the arrival time and let the schedule shift in small consistent increments as needed. Trying to do anything more complex based on live velocity data will just lead to a very unstable and confused traffic schedule, and I expect the traffic outcomes would be worse.

[cwrx777]

Hi @mxgrey,

Suppose robot1 and robot2 need to go through a waypoint to reach their respective destinations. And during deconflict, RMF decides to let robot1 go first and robot2 wait. However, while robot1 is moving to the waypoint, it got blocked by a dynamic object. Assuming robot live velocity is used, it will report a high remaining duration value for robot1. Will RMF be able to replan and let robot2 move first?

[kjchee]

Hi @mxgrey ,

When we update this estimated arrival duration, it will trigger and update the robot's itinerary cumulative delay, and subsequently shift the timestamp of the trajectory in robot itinerary.

So the question is, how accurate does this estimated arrival duration need to be?

What's the impact on traffic scheduling /traffic deconfliction, if the 'estimated arrival duration' accuracy be
a. accuracy <= 1 sec
vs
b. accuracy is a few seconds?

[mxgrey]

Assuming robot live velocity is used, it will report a high remaining duration value for robot1. Will RMF be able to replan and let robot2 move first?

In the current implementation of traffic waiting logic, robot2 will only consider replanning if robot2 itself has been waiting for 30s longer than it was supposed to. In the next generation of RMF I'd like to have logic for waiting robots to consider the current delays of their dependencies to consider replanning, but (1) that doesn't exist in the code today, and (2) by the time I introduce that capability, I think we'll have removed this time estimation API.

I think if you use live velocity data to estimate the arrival time then you'll easily get into pathological situations. If you use a speed that's close to zero and then do a typical first order estimate like dt = distance / speed then the arrival estimate dt would be extremely high. E.g. if the goal is 6m away and your current speed is 0.01m/s, then you'll estimate it will take 10 minutes to cover those 6 meters when realistically it will only take ~12 seconds (assuming a typical speed of 0.5m/s) once the obstacle has moved. If your live velocity drops all the way to exactly 0.0 then you'll end up with inf estimates.

So rather than using live velocity, if you want the most accurate arrival time possible then you'd need to first estimate how long the obstacle will be in the way and then calculate how long the robot will take after the obstacle has moved. Or if the robot is able to move around the obstacle, then estimate how long that new path will take.

But in practice I've found that doing a simple first order estimate based on the "nominal speed" of the vehicle (how fast the vehicle typically moves when there are no obstacles) and just updating it periodically based on the robot's current position will give the most stable and reliable outcomes. The traffic schedule is designed to handle this kind of periodic time shift very gracefully.

So the question is, how accurate does this estimated arrival duration need to be?

As mentioned in my last paragraph, the traffic schedule is designed to be very robust to time shifts in the itinerary updates. Being accurate to within a few seconds will easily suffice. Traffic dependencies are calculated with ~30s of leeway built in, so as long as a single arrival estimate update doesn't change the delay estimate by more than 30s, the traffic dependency system should remain stable. Even if you do occasionally have a delay estimate change of more than 30s the system should still be okay, you'll just start to get some undesirable traffic patterns if that happens excessively often.