[FEATURE] Integrate Autopilot package for drive-to-pose functionality

[tbowers7]

Is your feature request related to a problem? Please describe.
Use the Autopilot Package for drive-to-pose functionality.

Additional context
Figure out how to cleanly integrate this with other command functionality in RBSI.