Added the AStar classes and a test class in the vision package
Merge branch 'vision' of https://github.com/bvborisov/SDP_PC into vision