Better isolation of planning routine, refactored overall path planning/following routine, added markers

This branch was started in order to add a service server for retrieving planned path without directly driving them (e.g. to compare which item to retrieve first). It developed into a refactoring branch to better isolate the planning routine. I will rename this merge request and pull it into the current master. Then, I will start a follow-up merge request for finally adding this said service.

• Added planPath function
• Started refactoring replanning and proper reaction to obstacles
• Still work open in order to reduce number of indirections (mainly member variable states used in different functions which call each other)