Anbit/guidance #642

anbit-adhi · 2025-11-12T15:07:56Z

Addition:

From last year, this package was only for Adaptive LOS guidance. To make a LOS library that supports multiple types of LOS-based guidance and is robust enough to allow adding other guidance systems as well, my primary focus has been to implement more LOS-based methods first. As of now, I have added Proportional LOS and Integral LOS, in addition to the Adaptive LOS from last year.

Restructure:

I have also worked on restructuring the existing Adaptive ROS node and YAML file.

ros_node:
To create a system capable of changing the LOS method in the middle of a mission or segment, I needed to restructure the file named los_guidance_ros.cpp. Previously, this file only ran Adaptive LOS, but now it has been modified so that it can receive a method selection from a service called SetLosMode.srv and switch between all available LOS methods within a single running node. Together with our mentor Anders, we planned how the new structure should look and implemented it accordingly.

A new header file named types.hpp has been added, which holds all the shared data structures used in the code. All parts of the code are now within the same namespace, vortex::guidance::los, making it easier to reference and maintain consistency across different components.

YAML:
The parameter file has been restructured to use the default YAML format. This was done to clearly separate ROS parameters from code-based parameters. The remaining files such as CMakeLists.txt, package.xml, and the launch file have also been updated accordingly.

Test:

There was a test file for Adaptive LOS from last year. Following the same concept, I created equivalent tests for all the new LOS methods. Since there are now multiple test files, I also added a main test file that contains the main function to run all tests together.

For now, if we want to run a single test, it must be done using ROS2 commands in the terminal, but this can be improved later to make it simpler.

All test invocation:
colcon test --packages-select los_guidance --event-handlers console_direct+
colcon test-result --verbose
Example single-test invocation:
colcon test --packages-select los_guidance --ctest-args -R ProportionalLosTest

Plan further:

I plan to run more tests to ensure that all LOS methods work as expected. I also want to make further adjustments so that the ROS node can either remember or reset the Adaptive and Integral values. It would be useful to retain these values within the same segment but reset them when starting a new one.

Lastly, I plan to add more LOS methods, and if time permits, explore creating a library for other types of guidance methods as well.

Summary:

This PR completes the first stage of the LOS guidance library as planned. It introduces two new LOS methods (Proportional LOS and Integral LOS) and provides a clear separation between different methods. Additionally, the ROS2 node has been restructured to support runtime method switching, with updated configuration and the addition of unit tests for each method.

The node is ready to run, it builds and launches, but I still need to perform more testing to confirm that all methods work correctly and continue developing new LOS variants in the next phase.

for more information, see https://pre-commit.ci

chrstrom · 2025-11-12T15:48:39Z

just a control question, what's the use-case that requires one node to ad-hoc switch the LOS-flavor to use? I'm not too too familiar with the full system / intended use here

Andeshog

Good job so far!

Andeshog · 2025-11-12T18:04:11Z

auv_setup/config/robots/orca.yaml

+    services: # Maybe not the right place for this?
+      los_mode: "set_los_mode"
+


This is fine! Although if I am going to be picky I think it should be between topics and actions hehe

Andeshog · 2025-11-12T18:05:30Z

guidance/los_guidance/CMakeLists.txt

 if(BUILD_TESTING)
+  include(CTest)


Is the CTest include needed here?

Andeshog · 2025-11-12T18:11:23Z

guidance/los_guidance/include/los_guidance/lib/integral_los.hpp

+    // again i dont know if i should have them here or just in the functions
+    double pi_h_{};
+    double pi_v_{};
+    Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()};
+    Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()};


It is a fair point, and it makes sense to have just have them as input/output for the calculations

Andeshog · 2025-11-12T18:18:05Z

guidance/los_guidance/include/los_guidance/lib/types.hpp

+    PROPORTIONAL,  // 0
+    INTEGRAL,      // 1
+    ADAPTIVE       // 2


You could also explicitly set the integers

enum class ActiveLosMethod : uint8_t { PROPORTIONAL = 0, INTEGRAL = 1, ADAPTIVE = 2 };

Andeshog · 2025-11-12T18:23:55Z

guidance/los_guidance/src/lib/adaptive_los.cpp

+    const types::CrossTrackError& e) {
+    const double denom_h = std::sqrt(m_params.lookahead_distance_h *
+                                         m_params.lookahead_distance_h +
+                                     e.y_e * e.y_e);
+    const double denom_v = std::sqrt(m_params.lookahead_distance_v *
+                                         m_params.lookahead_distance_v +
+                                     e.z_e * e.z_e);
+
+    const double beta_dot =
+        m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e;
+    const double alpha_dot =
+        m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e;
+
+    beta_c_hat_ += beta_dot * m_params.time_step;
+    alpha_c_hat_ += alpha_dot * m_params.time_step;
+}


It's not immediately clear what e represents without looking at the function input. cross_track_error is a perfectly fine name, and it is very clear what it is 👍

Andeshog · 2025-11-12T18:29:20Z

guidance/los_guidance/src/lib/integral_los.cpp

+
+    int_h += e.y_e * m_params.time_step;
+    int_v += e.z_e * m_params.time_step;
+


Sidetracking a bit, but would be cool to have implementations of some common integrators like RK4 and forward euler in vortex-utils that could be used in cases like this👀

Andeshog · 2025-11-12T18:30:41Z

guidance/los_guidance/src/lib/proportional_los.cpp

+    const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9);
+    const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9);


This could be avoided by having a parameter validation in the constructor

Andeshog · 2025-11-12T18:33:44Z

guidance/los_guidance/src/los_guidance_ros.cpp

+    auto adaptive_los_config = config["adaptive_los"];
+    spdlog::info("A0");
+    auto params = AdaptiveLosParams{};
+    params.lookahead_distance_h =
+        adaptive_los_config["lookahead_distance_h"].as<double>();
+    spdlog::info("A1");
+    params.lookahead_distance_v =
+        adaptive_los_config["lookahead_distance_v"].as<double>();
+    spdlog::info("A2");
+    params.gamma_h = adaptive_los_config["gamma_h"].as<double>();
+    spdlog::info("A3");
+    params.gamma_v = adaptive_los_config["gamma_v"].as<double>();
+    spdlog::info("A4");
+    params.time_step = static_cast<double>(time_step_.count()) / 1000.0;
+    spdlog::info("A5");


I assume these prints are not supposed to be in the finished product 😆

Andeshog · 2025-11-12T18:36:09Z

guidance/los_guidance/src/los_guidance_ros.cpp

+
+    m_adaptive_los = std::make_unique<AdaptiveLOSGuidance>(params);


You are currently running two styles on member naming. I suggest you choose one of them 😄 FYI, the rest of the codebase use trailing underscore

Andeshog · 2025-11-12T18:40:34Z

guidance/los_guidance/src/los_guidance_ros.cpp

+    u_desired_ = common_config["u_desired"].as<double>();
+    spdlog::info("C1");
+    goal_reached_tol_ = common_config["goal_reached_tol"].as<double>();
+    spdlog::info("C2");
+    m_method = static_cast<types::ActiveLosMethod>(
+        common_config["active_los_method"].as<int>());
+    spdlog::info("C3");
+}


Would probably be wise to lock with the mutex here, as you are writing to data that is accessed in the while loop, to not run into any race conditions?

Andeshog · 2025-11-12T20:10:02Z

To fix the build errors you could try to add <depend>yaml-cpp</depend> to your package.xml so that it is (hopefully) installed with rosdep

anbit-adhi · 2025-11-13T12:04:56Z

Thank you @Andeshog for the comments, i will look into all of them after i am done with my exmas.

replying to @chrstrom :
As i have understood the main use-case that requires one node to switch LOS method during runtime is that different types of environment can need different type of methods. And those needs can change while the vehicle is already moving. In theory, Proportional LOS can be suitable for more still water, but if the drone meets a high current Adaptive LOS or integral LOS might be more suitable. Instead of stopping the mission, changing parameters, and relaunching the node, it can come in handy to be able to change LOS methods.
This also allows us to test and find out which LOS method can work in our situations and use that before hand but while testing it will be easier to change methods while we are running the node.

This explanation is from my understanding, so please do correct me if something doesn't feal right!

Anbit and others added 13 commits October 19, 2025 15:56

nye versjonen av guidance

2fb9d91

los test

064adef

merging origin/main into anbit/guidance

bbd8564

Resolve stash conflicts: keep my YAML; keep main's joystick node

e6ae145

fixing my error

4d22267

maths behind proportional LOS

3f3e671

refactor: change los structure

2688e27

refactor: remove unused los package

798542a

fix: fix ignore lib

3bb8926

Test: add test files for ALos and Plos

b7f63a9

complete the restructuring of ros node

ee303e7

Complete los switchin

c687343

[pre-commit.ci] auto fixes from pre-commit.com hooks

03560cf

for more information, see https://pre-commit.ci

Andeshog reviewed Nov 12, 2025

View reviewed changes

		services: # Maybe not the right place for this?
		los_mode: "set_los_mode"


		int_h += e.y_e * m_params.time_step;
		int_v += e.z_e * m_params.time_step;

		const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9);
		const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9);


		m_adaptive_los = std::make_unique<AdaptiveLOSGuidance>(params);

Anbit/guidance #642

Are you sure you want to change the base?

Anbit/guidance #642

Uh oh!

Conversation

anbit-adhi commented Nov 12, 2025

Addition:

Restructure:

Test:

Plan further:

Summary:

Uh oh!

chrstrom commented Nov 12, 2025

Uh oh!

Andeshog left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andeshog commented Nov 12, 2025

Uh oh!

anbit-adhi commented Nov 13, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

anbit-adhi commented Nov 13, 2025 •

edited

Loading