latest work on rosified planner

This is working on issues #36 and #22