[FEATURE] Enable servo control from gRPC server

### Description

Introduced in #82 , we are able to publish to the servo command topics, but we still need to: 1) switch the controllers 2) enable the servo 3) do the opposite to disable it.

### References

_No response_