Almost done testing for pathfinding

Seems to work as it should for pathfinding (atleast the AStarPlanner,
returning neighbors is the graphs responsibility and how it's done may
vary ofc.
TODO:
* Replace raw-pointers with smart-pointers (or atleast declare as
unique-unique in the PlannerGraphBase).
* Update the planner-UML