Formation Path Controller
This marks the first complete version of the local planner of the formation path controller.
This is the first version where both FormationPathPlanner und FormationPathController are implemented and working.
Missing:
- Reconfiguration during drive