Waypoint node

[AbubakarAliyuBadawi]

No description provided.

[Andeshog]

Open a PR in vortex msgs so that we can test this

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 199 lines in your changes missing coverage. Please review.

Project coverage is 6.04%. Comparing base (54b5a4a) to head (5826820).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
mission/waypoint_manager/src/waypoint_manager.cpp	0.00%	183 Missing ⚠️
...ion/waypoint_manager/src/waypoint_manager_node.cpp	0.00%	16 Missing ⚠️

Additional details and impacted files

@@           Coverage Diff            @@
##            main    #566      +/-   ##
========================================
- Coverage   6.61%   6.04%   -0.57%     
========================================
  Files         37      39       +2     
  Lines       2117    2316     +199     
  Branches      50      64      +14     
========================================
  Hits         140     140              
- Misses      1977    2176     +199     

Flag	Coverage Δ
unittests	6.04% <0.00%> (-0.57%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ion/waypoint_manager/src/waypoint_manager_node.cpp	0.00% <0.00%> (ø)
mission/waypoint_manager/src/waypoint_manager.cpp	0.00% <0.00%> (ø)

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[chrstrom]

General comment on this file:

Because of the independent main blocking loop, there's a lot of locking/unlocking going on, and I can't immediately say that it's 100% correct.

Question is then: Do you really need this to run async, or could you fundamentally restructure the code so that you avoid the mutual exclusion requirement, or even use condition variables in a producer/consumer pattern?

[AbubakarAliyuBadawi]

I have refactor the code to add a dedicated worker thread that uses condition variables instead of a busy-wait loop, can you have a look.

[Andeshog]

tldr; it crashes due to throwing runtime error upon reaching the first waypoint

[Andeshog]

Be careful with this since the reference filter is not dependent on the drone position. The reference filter will go until it is close enough to the target point, and then it will finish and return success. I tried to test your code and when the drone reaches the first waypoint both the reference filter node and the waypoint manager node crashes. I would suggest that you let the respective guidance systems handle when they reach the waypoint by just waiting for the action to return success.

[AbubakarAliyuBadawi]

I see... I will look into that, can you tell me which specific error the node is throwing?

[AbubakarAliyuBadawi]

I refactor the code, can you try again?

[Andeshog]

Now nothing seems to happen when the goal request is sent to the waypoint manager. I would advise you to run full tests of your own to make sure things work as intended before pushing.

[chrstrom]

Regarding my questions about the multithreading here, I wrote an MVP for the functionality that is desired here, and found that you really can just run this with a single-threaded executor? I also got the complexity down by using a service server to take waypoints in (which I think is the appropriate choice here, given that you don't really need to signal to the provider of waypoints that waypoints have been reached - thats the waypoint manager's (and guidance system's) job).

A quick outline:

1. std::dequeue for internal storage (under the assumption that we don't want to keep waypoint history)
2. service server that pushes back incoming waypoints to the queue - and additionally sends the next waypoint is no goal is in progress
3. in the method that sends the next waypoint, i used a local variable (Client ... ::SendGoalOptions()) with goal response and result callbacks as inline lambdas, invoking 4.
4. a method to handle the logic of the action server response - a boolean indicating success or failure, which then calls 3. again

While this solution is not "100%" - i.e. the action client "feedback" mechanism is not used at all, and the logic I implemented as a test in 4. is very barebones, I think the point remains to simplify and single-thread.

TLDR: Use a service server for incoming waypoints, and single-thread the waypoint manager

[AbubakarAliyuBadawi]

Regarding my questions about the multithreading here, I wrote an MVP for the functionality that is desired here, and found that you really can just run this with a single-threaded executor? I also got the complexity down by using a service server to take waypoints in (which I think is the appropriate choice here, given that you don't really need to signal to the provider of waypoints that waypoints have been reached - thats the waypoint manager's (and guidance system's) job).

A quick outline:

1. std::dequeue for internal storage (under the assumption that we don't want to keep waypoint history)

2. service server that pushes back incoming waypoints to the queue - and additionally sends the next waypoint is no goal is in progress

3. in the method that sends the next waypoint, i used a local variable (Client ... ::SendGoalOptions()) with goal response and result callbacks as inline lambdas, invoking 4.

4. a method to handle the logic of the action server response - a boolean indicating success or failure, which then calls 3. again

While this solution is not "100%" - i.e. the action client "feedback" mechanism is not used at all, and the logic I implemented as a test in 4. is very barebones, I think the point remains to simplify and single-thread.

TLDR: Use a service server for incoming waypoints, and single-thread the waypoint manager

You mentioned using a service server instead of an action server. Does this mean we don't need to support cancellation of waypoint sequences once they're submitted? What about cases where a mission needs to be aborted?

[AbubakarAliyuBadawi]

Could you share the MVP implementation you created that will help me understand the simplified architecture better since I am new to C++

[Andeshog]

Could you share the MVP implementation you created that will help me understand the simplified architecture better since I am new to C++

It is in the branch from easter testing: https://github.com/vortexntnu/vortex-auv/blob/easter-test-2025/mission/waypoint_manager/src/waypoint_manager2.cpp

[Andeshog]

Regarding my questions about the multithreading here, I wrote an MVP for the functionality that is desired here, and found that you really can just run this with a single-threaded executor? I also got the complexity down by using a service server to take waypoints in (which I think is the appropriate choice here, given that you don't really need to signal to the provider of waypoints that waypoints have been reached - thats the waypoint manager's (and guidance system's) job).
A quick outline:

1. std::dequeue for internal storage (under the assumption that we don't want to keep waypoint history)

2. service server that pushes back incoming waypoints to the queue - and additionally sends the next waypoint is no goal is in progress

3. in the method that sends the next waypoint, i used a local variable (Client ... ::SendGoalOptions()) with goal response and result callbacks as inline lambdas, invoking 4.

4. a method to handle the logic of the action server response - a boolean indicating success or failure, which then calls 3. again

While this solution is not "100%" - i.e. the action client "feedback" mechanism is not used at all, and the logic I implemented as a test in 4. is very barebones, I think the point remains to simplify and single-thread.
TLDR: Use a service server for incoming waypoints, and single-thread the waypoint manager

You mentioned using a service server instead of an action server. Does this mean we don't need to support cancellation of waypoint sequences once they're submitted? What about cases where a mission needs to be aborted?

We can still send cancellation request directly to the guidance server. It will then return false (for success) which signals to the waypoint manager that it should not feed next waypoint.