Support for different goals in different frames

Use header frame instead of world_frame for goal