Duplicate A* Code should be resolved

[dmcconachie]

#16 and #17 added in a duplicate A* implementation inside of arc_helpers.hpp alongside the code in simple_astar_planner.hpp