Fixed errors and noobish c++ misstakes

Fixed some compiler-errors (used templates a little bit wrong), and
tested a solution with reusable nodes in PlannerGraphBase (renamed from
IPlannerGraph). Is lambda an efficient solution? Feels a little bit
itchy!
TODO: Fully implement PathGraph and test.