TO DO: Controller among obstacles + Map