Avoid robot backtracking when a RobotCommandHandle::follow_new_path() is triggered
Yadunund opened this issue · 6 comments
rmf_fleet_adapter uses rxcpp for reactively generating a new plan for the robot on a number of occasions including during traffic negotiation and when itinerary delays accumulate past a threshold. As a response, the RobotCommandHandle::follow_new_path() may be triggered to instruct the robot to follow the updated itinerary. However, between the time of the interruption and when the implementation of follow_new_path() sends an instruction to the robot, the robot would have continued moving along its previous path. As a result when the robot follows the new set of waypoints, it "backtracks", ie moves to the first position in the path which is slightly behind the robots current position, before continuing along the remaining path. Presently the workaround for this is to perform some filtering in the waypoints received.
Using this ticket to discuss potential solutions for this problem.
One easy solution would be to request the robot to stop immediately whenever the interrupt is triggered. The RobotUpdateHandle could accept a callback from users that would get their robots to stop when triggered. However, this is a big change in the behavior as I think one of the goals with the current implementation is to have the robots continue along their plans while the negotiations occur.
Another possible approach is to have the planner perform some kind of trajectory tuning/optimization based on the kinematics of the fleet.
Looking for feedback on these ideas along with other suggestions.
I think post-processing of the plan based on the latest robot state is the best approach. We would compare the plan's expectations to our knowledge of the latest physical reality of the robot and just prune some waypoints off the front of the trajectory if the robot has already passed them.
One difficulty with this is identifying cases where the robot does need to backtrack, e.g. to escape a conflict with another robot. In those cases, pruning the start of the plan would lead to a deadlock. This issue has been the primary blocker that's prevented us from taking action on this problem.
I think when we have the event-driven traffic system that can recognize the purpose behind each wait, we can leverage the event expectation system to identify when the robot needs to backtrack versus when it's safe to prune the front of the trajectory.
Thanks for the valuable insights @mxgrey. Could we look at the time property of the first waypoint in the new path to determinate whether the intention is to wait at the previous location? Maybe check if the if the wait duration is above a certain threshold? I assume if the robot is not meant to wait, the first waypoint in the new path will have time very close to now?
Looking at time alone would not be reliable since it's possible that the plan was generated some time in the past and needs your robot to back off to let another robot pass. If you ignore the initial waypoints because they were targeting "some time ago" then you might continue to block the path of the other robot (while it most likely also blocks yours), leaving both in a deadlock.
Granted, if planning and negotiation works fast enough then the problem that I described shouldn't happen, but I don't think critical systems should rely on an assumption about how fast any sub-process will manage to run.
I'll mention here that the new event-driven traffic system can actually handle this issue in a much more elegant way.
Now that it's okay to ignore the timing of the waypoints in each path command, that also means you can try to interpolate the robot's initial progress along each new path command, and simply have the robot continue driving forward along the path, ignoring any waypoints that fall behind the robot.
That wasn't allowed before because sometimes the path command would have included a backup sequence that needs to be followed to move the robot out of the way of another robot before it can proceed. In the new system if a backup sequence is needed, then that backup sequence will be the entire path command, and the robot won't receive a command for the remaining path until all other robots are out of the way.
Thanks for that insight! The tricky part previously with interpolation has been the estimation of the distance the robot travelled in the time duration between a replan interrupt and the when the fleet adapter submits a navigation request to the robot. And depending on the goal tolerance of the robot's navigation stack, the robot may or may not backtrack depending on how far it is from the interrupt location.
But with the new system, its great to know that we can ignore the waypoints that fall behind the robot 👍🏼