Changes from rob@work

This is a wrap_up/code_commit after rob@work's ICRA/ROSCON demo playing back the changes to ros-planning