Can now determine direction of travel for VFH. Need to update the cmd_vel.rotation.z to the new direction of travel