Refactoring getting the state/command handle

[tonynajjar]

Describe the solution you'd like As proposed here https://github.com/ros-controls/ros2_controllers/pull/345#discussion_r922725723, the idea is make a utility function to retrieve a state/command handle from the state/command interfaces.

Inputs of that function: interfaces vector, name to look for, interface name (type) to look for Returns: std::optional