Refactor goal pose

All right, from here we should start moving to a separate plugin ;)