Goals now publish on move_base