[Task] Extend waypoint functionality to include orientation for DP

[Q3rkses]

Description of task

Extend the waypoint manager GUI and backend so that waypoints can include desired orientation in addition to position. Current GUI supports position-only waypoints (x,y,z); extend to (x,y,z, roll, pitch, yaw). The GUI should accept RPY for user convenience.

Suggested Workflow

1. Align with DP/reference-filter owners on frames, topics, and I/O (world frame for positions/orientations, base_link for the vehicle).
2. GUI changes: add Roll Pitch Yaw inputs (with degree/radian toggle), validation, and display.
3. Message/schema: standardize on geometry_msgs/PoseStamped (or equivalent) with {position, orientation};
4. Testing: create scenarios for static-orientation alignment and move+orient maneuvers; record convergence metrics.

Specifications

Behavior

• Waypoint structure: (x, y, z, roll, pitch, yaw) input in GUI; transport as Pose/PoseStamped
• Frames: positions and orientations specified in world/map (NED). Vehicle orientation refers to base_link’s pose at waypoint.

UI/UX

• Roll Pitch Yaw inputs with unit selector (deg/rad)

Topics / Parameters (configurable)

• Subscribe (optional): //odometry for previewing current pose and showing it directly in the UI/UX.
• Publish: //reference_filter/waypoint (PoseStamped) or existing waypoint topic with orientation added.

Suggested testing

• Static orientation tests (DP no-translation):
  • From varied initial attitudes, command (x,y,z fixed) with different (r,p,y).

Contacts

Waypoint contact person? — @kluge7 / @Q3rkses
Software lead — @jorgenfj

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)