Introducing ResponsiveWait task phase

[mxgrey]

This PR introduces the ResponsiveWait task phase. This task phase can be used any time a robot needs to wait somewhere that it might interfere with the traffic of other schedule participants.

While running the ResponsiveWait phase, a robot will try to sit on the designated waypoint, but will automatically move out of the way of other traffic and will participate in traffic negotiations.

In this PR, we also have any idle robots automatically engage in a ResponsiveWait phase, which will automatically be canceled when they are assigned a new task.

Note that this change means that if two robots are both idle on locations where they have overlap in their footprints/vicinities, then they will likely enter an endless cycle of negotiating with each other. That problem will be addressed when the parking spot reservation system is completed. In the meantime, users are advised to avoid situations where robots are idle on locations where they conflict with each other.

[mxgrey]

This PR is useful for testing these changes: osrf/rmf_demos#213

[Yadunund]

Tested out this feature and it works great! I have one concern.

Do you think it will be worthwhile to have a function in RobotUpdateHandle to toggle on/off the ResponsiveWait behavior? In my testing I have a map with parallel lanes where a robot is parked on one of the lanes. Usually another robot has no issue traveling down the other lane. But now with this feature, it forces the robot take a much longer detour because the vicinity of the parked robot overlaps with the other lane. This longer detour was not originally accounted for when the task assignment planner derives its solution so there may be a rare case where a charging task may have been required prior to this task. But this is definitely an edge case.

Another case could be when a parked robot in ResponsiveWait state negotiates with another robot and then gets an itinerary to back up into a waypoint that happens to be a docking point. Then, an unnecessary docking maneuver will be performed by this robot.

[mxgrey]

In my testing I have a map with parallel lanes where a robot is parked on one of the lanes.

With the addition of this behavior, I think we'll need to reconsider some maps and how they're configured. I think there's a tendency to have needlessly large vicinities for robots, and that's something we should try to cut back on. Cutting down on the vicinity sizes should eliminate most of these idle robot interference issues.