Add dynamic detection when a reconfiguration is prefered

[Heenne]

Detection could detect following cases:

• Path planner cannot reach goal. (Solution: Try planning path with alternative formation. If succeded, then get point where the previous formation was stuck on the new plan. Define that until this point the formation must be changed.)
• Path might be significantly shorter than without reconfiguration. This would just be a second planning with a different configuration (and costmap, this must update. Costs time). This is time costly