Added option to only use rangefinder forward, close #32