Pathplanning debuged and enhanced

Signed-off-by: eldr4d <nikofinas@gmail.com>
Merge branch 'master' of github.com:kouretes/Monas
Remove debug code from behavior

Signed-off-by: eldr4d <nikofinas@gmail.com>