Merge the refactor/sergio+ed branch for action_server