Add grid map and better handling of obstacles in maps