[TASK] Velocity Controller

[Andeshog]

Description of task

Implement a controller that controls velocity, heading and pitch. The type of controller should be discussed. See 434 for controller ideas.

Suggested Workflow

• If needed, read up on the chosen method.
• Implement and tune the controller using the simulator.
• Test different scenarios in the simulator.

Specifications

• Wrench Message for the output.
• "thrust/wrench_input" for the topic name.

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)