Anbit/guidance

[anbit-adhi]

This PR adds the los_guidance package and integrates it into the autopilot setup for waypoint-based guidance of the AUV. The node computes guidance references from the current vehicle state and a target waypoint, and sends the desired yaw, pitch, and surge to the downstream controller.

The package also has overwritten the old simulator test with need changes for it to work with the new guidance. Most of the implementation details, usage, and configuration are described in the README.

[vortexuser]

first of round of reviews

[jorgenfj]

I think it makes sense to move most of the state management out of the ros node. Could follow a similar setup to the "waypoint_follower.cpp" file in the reference filter

[jorgenfj]

Maybe make this a config param?
Also should make the member variable name more explicit by naming it time_step_ms_

[jorgenfj]

This should be a util function

[jorgenfj]

Could also consider wether we should move this into vortex_utility_nodes in vortex_utils to avoid creating a dependency on velocity/odom in this node. Either way, the odom sub creating should be guarded by a debug flag so we dont subscribe uncessary

[jorgenfj]

Suggestion: replace the four unique_ptr members + ActiveLosMethod enum with std::variant

Currently LosGuidanceStateManager holds all four LOS implementations alive simultaneously and uses
a separate method_ enum + switch statement to dispatch:

types::ActiveLosMethod method_{};
std::unique_ptr adaptive_los_{};
std::unique_ptr integral_los_{};
std::unique_ptr proportional_los_{};
std::unique_ptr vector_field_los_{};

Since all four types share the same calculate_outputs(const types::Inputs&) → types::Outputs
interface, this could be collapsed into a single:

std::variant<ProportionalLOSGuidance, IntegralLOSGuidance,
AdaptiveLOSGuidance, VectorFieldLOSGuidance> los_method_;

The dispatch switch in calculate_outputs() then becomes:

std::visit([&](auto& los) { outputs = los.calculate_outputs(inputs_copy); }, los_method_);

The main complication with variant is that set_los_method() can no longer just set an enum —
switching types requires constructing the new object with its parameters. This is resolved by tying
method selection to goal initialization: initialize_goal() re-parses the YAML config, reads the
active method, and emplaces the right alternative with fresh parameters. This also means config
changes (lookahead distances, gains, etc.) take effect at the start of each new segment without a
restart.

A nice side effect: constructing a fresh instance implicitly resets stateful methods
(AdaptiveLOSGuidance, IntegralLOSGuidance), so the unconditional adaptive_los_->reset() call in
initialize_goal() — which currently fires regardless of the active method — goes away naturally.

^^from mr. claude.

If you go for variant you should store the yaml file path as a member. And the 4 los setters in the constructor should be replaced by a single function set correct los method based on the active los config param

[jorgenfj]

Rename to GuidanceOutputs or something more explicit

[jorgenfj]

Same here. Maybe make the name more explicit

[jorgenfj]

🤡

[jorgenfj]

Same comment as previously with the ActiveLosMethod initialization. This can cast to values outside the valid ENUM range. Also there is no guarantee that the vortexmsg value matches the enum values. Should use an explicit conversion function