Obstacle detection and VFH planning.

Signed-off-by: Thomas Hines <hineta64@gmail.com>