[FEATURE]: Implement support for a GUIDED mode hardware interface

[evan-palmer]

Feature Type

Adding new functionality to the project

Problem Description

The ArduSub GUIDED mode provides access to the ArduPilot velocity & position PID controllers. With a suitable hardware interface, this can be utilized from higher-level controllers in a cascading control framework.

Feature Description

A new hardware interface should be implemented that provides access to position and velocity control via GUIDED mode. Two command modes should be implemented for each type of control. To facilitate state feedback control, the hardware interface should subscribe to the position and velocity state messages published by the flight controller and publish those as state interfaces.

Alternative Solutions

On one hand, this control strategy could be accomplished just by interacting with ArduSub directly. However, this doesn't facilitate integration with ros2_control which is the primary motivator for this feature.

Additional Context

No response