Permanent error state after error reached

[seanrivera]

After an error lead to 2 robots getting too close to each other to finish route planning the system seems to be in a permenant state where it is unable to plan any routes going forward. Even moving the robots back to their starting positions and restarting the planning does not fix it. As far as I can tell nothing short of completely relaunching the system will fix this.

The error is: 'Multi Robot Router: Goal of robot 0 "robot_0" was not found'
'Multi Robot Router: Goal of robot 1 "robot_1" was not found'
and then the success information gives success as -nan.

[maxbader]

The goals and the robots must be sufficiently close to a knot on the voronoi graph in order to start the planner