A hacked proof of concept version of a planner

It's running RRT. 
rqt_graph output attached in images and diagrams.