Implemented obstacle detection. Currently freezes the thread without segfault for no reason, so commented out.