Trying to create a set class
A* implemented. Is very slow for maps bigger than 5*5, so I suspect (hope) that's some buggy implementation's fault
Merge branch 'mapping_theta' of http://github.com/TheiaRobo/Theia into mapping_theta