[TASK] Guidance node for DP

[Andeshog]

Description of task

Given a set of 3D waypoints, generate a path that goes through each waypoint. Then define a way to make Orca follow this path by publishing guidance messages. There are a lot of options for both path generation and path following, so this needs to be researched before starting the task.

Ideas:

• Virtual target following
• Extend Hybrid Path Guidance to 6 degrees of freedom.
• Fossen's Reference Model, slide 11 and 12.
• Line of Sight (LOS)

Suggested Workflow

• Research on the chosen method.
• Implement and test by plotting 3D plot of path + waypoints and of each degree of freedom over time.
• Test in the simulator.

Specifications

• Use Float32MultiArray as the message type, or define a custom message type if that is prefered.
• The node should have two separate modes for orientation:
  • Heading and pitch that point along the path or towards the next waypoint.
  • Heading and pitch that point towards a specific point in 3D space.

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)

[chrstrom]

Historical context: Aksel's VTF (code linked above and described in his thesis) worked REALLY well for Beluga Mk2 at robosub 🚀 The code is as all master's thesis code, soo if you end up going for VTF, might as well rewrite it