Cleaning up navigation node code

See merge request !27

Added publisher for driven path; Fixed strange scaling based on angle difference, resulting in speeding up instead of slowing down in some cases

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

See merge request !24

Call performNextMove in idleProcess instead of pose callbacks; better update of current m_distance_to_target;Added target marker

Decrease distance to obstacles when planning in order to avoid driving way to much on small position changes

major restructuring of how to plan paths; also use current laser scan for obstacle avoidance for long term path checking, extended num waypoints checked in advance; some additions in optional map output visualization for obstacle checking