Colission check with path and known obstacles implemented.