Major bugfixes in the planner

It now properly checks collisions.
The current path planner is very slow, supposedly because of the multiple calls to ros tf listener in each iteration.
Trying to fix that.
Merge branch 'master' of https://github.com/valeriovarricchio/DRC_catkin
Merge branch 'master' of https://github.com/valeriovarricchio/DRC_catkin