Add A* path finding algorithm (2) Fix implementation, add Gaussian to travel further from obstacles
Add A* path finding algorithm (2) Fix implementation, add Gaussian to travel further from obstacles

stheophil/MappingRobot2,stheophil/MappingRobot2,stheophil/MappingRobot2,stheophil/MappingRobot2,stheophil/MappingRobot2