Improvements on vfh
catkin_make fail on smooth_planner so: rm it
Disable debug costmap publishing
Now using bresenham alg to check costmap cells
Commented debug costmap stuff.  It takes .050ms!