[TASK] Guidance node for autopilot

[Andeshog]

Description of task

Write a node that gives instructions to the velocity controller including surge velocity, pitch angle and heading angle. The chosen method should be discussed.

Suggested Workflow

• Research on the chosen method.
• Implement and test in the simulator.

Specifications

• Use Float32MultiArray or a custom message for the output.

Contacts

• @Andeshog

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)