Feat/auto refactor

[Q3rkses]

auto changed from easter testing / tacc / div other stuff compressed to minimal commit

[CurryMonsters]

the autonomy stack on rework/landmark-server is built around the waypoint manager there, and every movement node in vortex_bt_nodes goes through it.

Some of this is used in the TAC challenge, and some is used by our new stack with the BT. Jørgen also made some of the changes, like keep_altitude / desired_altitude, which are nice features that we use.

Needed by the BT stack:

Goal frames: WORLD, BODY_RELATIVE and WORLD_RELATIVE, resolved once from the pose at goal start through frame_resolver (used by MoveRelative, Search and Turn)
Outcomes: SUCCEEDED, PREEMPTED, CANCELED, INVALID_GOAL, REFERENCE_FILTER_ABORTED, with message, reached_index and current_index in the feedback. This helps the bt know wgy node failed etc.
GoToFrame resends goals while it follows a frame, so a replaced goal must come back as PREEMPTED, not as a failure
Bad goals (NaN/inf poses, unknown frames, a relative frame before we have a pose) are rejected as INVALID_GOAL without killing the goal that is already running

Waypoint modes to keep

FULL_POSE, ONLY_POSITION, FORWARD_HEADING, ONLY_ORIENTATION, POSITION_AND_YAW, XY_AND_YAW, ONLY_Z
SetDepth uses ONLY_Z, Turn and LookAtFrame use ONLY_ORIENTATION, GoToFrame uses POSITION_AND_YAW

Also keep

keep_altitude / desired_altitude
Persistent goals
waypoint_addition with overwrite/priority
current_waypoint debug publisher (the sim draws it as the waypoint arrow)

Some of these are used by the pipelinefollowing, and are needed