Feat/waypoint manager by jorgenfj · Pull Request #645 · vortexntnu/vortex-auv

jorgenfj · 2025-12-07T19:09:12Z

vortexntnu/vortex-msgs#35
Added mode logic for waypoints in reference filter.
Added waypoint manager + some basic tests

Andeshog

Would be nice to see a four corner test (similar to what we did in easter testing) addition to the integration tests.

Andeshog · 2025-12-07T20:17:48Z

+        std::bind(&WaypointManagerNode::handle_waypoint_goal, this,
+                  std::placeholders::_1, std::placeholders::_2),
+        std::bind(&WaypointManagerNode::handle_waypoint_cancel, this,
+                  std::placeholders::_1),
+        std::bind(&WaypointManagerNode::handle_waypoint_accepted, this,
+                  std::placeholders::_1));


Generally, consider using lambda instead of std::bind.

Andeshog · 2025-12-07T20:35:01Z

+    bool persistent_action_mode_{false};
+    bool priority_mode_{false};
+
+    ReferenceFilterAction::Feedback latest_ref_feedback_;
+    bool have_reference_pose_{false};
+    bool cancel_in_progress_{false};


persistent_action_mode_active_
priority_mode_active_
has_reference_pose_
is_cancel_in_progress_
perhaps?

Andeshog · 2025-12-07T20:36:42Z

+
+    std::uint64_t mission_id_ = 0;


Cant wait to see mission number 18,446,744,073,709,551,615 😂

Andeshog · 2025-12-07T20:41:49Z

+
+    (void)goal_handle;  // suppress unused variable warning


could also just do

rclcpp_action::CancelResponse WaypointManagerNode::handle_waypoint_cancel( const std::shared_ptr<WaypointManagerGoalHandle> /*goal_handle*/)

Andeshog · 2025-12-07T20:55:02Z

+    Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal);
+


Why is stamped removed?

The function did not use the header, so no need to pass more than what is required

vortex utils ros conversion here instead?

Andeshog · 2025-12-07T21:17:51Z

+        case vortex_msgs::msg::Waypoint::FORWARD_HEADING: {
+            double dx = r_in(0) - x_(0);
+            double dy = r_in(1) - x_(1);
+
+            double forward_yaw = std::atan2(dy, dx);


2 things:

No reason to use both heading and yaw in this block, choose one.

r_ and x_ are really poor names. Would be better off using something like reference_pose and current_drone_state. Would not be bad to integrate some of the new structs in vortex utils either, but that is probably out of scope for this PR

'out of scope for this PR' my favorite words

Issue material 👨‍🦯

Andeshog · 2025-12-07T21:24:24Z

+        case vortex_msgs::msg::Waypoint::ONLY_POSITION:
+            r_out(3) = x_(3);
+            r_out(4) = x_(4);
+            r_out(5) = x_(5);
+            break;


Why isn't logic like ONLY_POSITION and ONLY_ORIENTATION handled in the motion controller(s) instead? Also with this you risk locking the desired orientation in awkward ways, instead of just not controlling said DOFs. If the locking behavior (fixing the desired value of one or more DOF) is something that is desired, that could easily be handled in the controller(s) in the same logic segment

I do think its fine that the wp manager/guidance controls which mode and so on, but the controller(s) should probably also know what mode it is performing

The locking is desired for this use case.

I agree that the mode logic could be handled in the controller, but this setup currently works as a MVP, and fits nicely within the scope of the action.
I feel the guidance and controller system could use a refactor with a "main orchestrator" or something, and this mode logic can be revamped then. The waypoint manager could potentially be incorporated into this "main orchestrator" and be extended to support LOS/velocity controller as well.

locking the drone to a desired orientation could be something that the state machine requests and guidance system does i don't see the point of the (DP) controller differenciating between locking orientation and locking pose it is the same thing but simpler, albeit could be better tuned to some extent if we separate them.

The use case in this situation is more that we do not really want to change the position which would require the state machine to remember its last waypoint, recover the (x,y,z) coordinates and then send the same waypoint but with changed orientation which seemes tedious. Same goes for position only vice versa.

The thing that could be something to discuss is if we want the state machine to only communicate its desires with the orchistrator/manager node, otherwise we could make the state machine directly send requests to DP guidance about locking orientation.

Possible to just let each dof be addressable (i.e. Eigen::Index, strongly typed) and let whatever thing thats managing task execution lock whatever axes as required? (could also alias "common" things like position only, orientation only, z only, etc, etc). That way you keep the controller "dumb" in the sense that it has no knowledge of "mode", just "which axes are currently constrained". (I don't know anything about the current dp code so correct me if this is already the case 🔥)

Andeshog · 2025-12-07T21:30:28Z

+    tf2::Quaternion q;
+    q.setRPY(fb.reference.roll, fb.reference.pitch, fb.reference.yaw);
+
+    pose.orientation = tf2::toMsg(q);


seems to me this would make a nice vortex_utils function.

vortexntnu/vortex-utils#19

Andeshog · 2025-12-08T07:07:48Z

+rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal(
+    const rclcpp_action::GoalUUID&,
+    std::shared_ptr<const WaypointManager::Goal> goal) {


A better way of handling unused parameters:

rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal( const rclcpp_action::GoalUUID& /*goal_uuid*/, std::shared_ptr<const WaypointManager::Goal> goal)

Q3rkses

lazy review

Q3rkses · 2025-12-11T13:34:51Z

+        case vortex_msgs::msg::Waypoint::ONLY_POSITION:
+            r_out(3) = x_(3);
+            r_out(4) = x_(4);
+            r_out(5) = x_(5);
+            break;


locking the drone to a desired orientation could be something that the state machine requests and guidance system does i don't see the point of the (DP) controller differenciating between locking orientation and locking pose it is the same thing but simpler, albeit could be better tuned to some extent if we separate them.

The use case in this situation is more that we do not really want to change the position which would require the state machine to remember its last waypoint, recover the (x,y,z) coordinates and then send the same waypoint but with changed orientation which seemes tedious. Same goes for position only vice versa.

The thing that could be something to discuss is if we want the state machine to only communicate its desires with the orchistrator/manager node, otherwise we could make the state machine directly send requests to DP guidance about locking orientation.

Q3rkses · 2025-12-11T14:01:57Z

+    pose.position.x = x
+    pose.position.y = y
+    pose.position.z = z
+    half = yaw * 0.5


why not quat to euler?

jorgenfj · 2025-12-17T16:05:36Z

@Andeshog @kluge7 I think we might have to move the new tests to a separate repo or change the workflow. Right now some of the workflows does not have stonefish in their dependencies.repos and when running colcon test, the tests that depend on stonefish fails.

Andeshog · 2025-12-17T16:13:11Z

@Andeshog @kluge7 I think we might have to move the new tests to a separate repo or change the workflow. Right now some of the workflows does not have stonefish in their dependencies.repos and when running colcon test, the tests that depend on stonefish fails.

The nodetests/launch tests should not depend on stonefish? Those tests are only testing the ros part for a node. Integration/system tests can stay as they are (the bash scripts) IMO, from this comment.

codecov · 2025-12-18T12:13:51Z

Codecov Report

❌ Patch coverage is 50.20243% with 123 lines in your changes missing coverage. Please review.
✅ Project coverage is 35.63%. Comparing base (54caaa7) to head (f1f181a).
⚠️ Report is 78 commits behind head on main.

Files with missing lines	Patch %	Lines
...sion/waypoint_manager/src/waypoint_manager_ros.cpp	51.47%	77 Missing and 22 partials ⚠️
...e/reference_filter_dp/src/reference_filter_ros.cpp	44.18%	20 Missing and 4 partials ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main     #645       +/-   ##
===========================================
+ Coverage   24.01%   35.63%   +11.62%     
===========================================
  Files          38       38               
  Lines        2286     2256       -30     
  Branches      612      686       +74     
===========================================
+ Hits          549      804      +255     
+ Misses       1588     1273      -315     
- Partials      149      179       +30

Flag	Coverage Δ
unittests	`35.63% <50.20%> (+11.62%)`	⬆️

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

Files with missing lines	Coverage Δ
...e/reference_filter_dp/src/reference_filter_ros.cpp	`72.94% <44.18%> (+72.36%)`	⬆️
...sion/waypoint_manager/src/waypoint_manager_ros.cpp	`51.47% <51.47%> (ø)`

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

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

Andeshog · 2025-12-19T07:40:42Z

+    const geometry_msgs::msg::Pose goal =
+        goal_handle->get_goal()->waypoint.pose;
+    uint8_t mode = goal_handle->get_goal()->waypoint.mode;
+    double convergence_threshold =
+        goal_handle->get_goal()->convergence_threshold;


I would try to use auto here, and elsewhere where it makes sense. It avoids visual clutter and potential accidental implicit conversions. However, do not use auto with Eigen types (eigen common pitfalls)

Andeshog · 2025-12-19T07:41:50Z

+
+if(BUILD_TESTING)


unnecessary if here?

Andeshog

lgetm

* waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * removed unnecessary if here

github-actions · 2026-02-18T21:28:45Z

🎉 This PR is included in version 2.6.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

jorgenfj added 24 commits November 24, 2025 21:34

waypoint mode handling

38f1fdb

update tests with new message

f3fc24b

initial package setup

d5bbaca

ros interface function declaration

4ff98d5

node setup

4c30faa

working prototype

dae4fa3

reentrant cb, multithreaded

2b39764

single threaded impl

676c54f

conv threshold action goal

ebb220c

default thresholdref conv value

490e307

removed switching logic

900f2af

removed timer execution

7065c6a

sim test utils

7209ce3

waypoint_manager_test setup

87ed751

no rendering test arg

1818953

waypoint tests, timeout error

8248353

test refactor

3aaf6f9

format

8784b2e

rename utils package

4d73ecb

test suite and description

76183bc

first waypoint test

b82db78

removed unused function

ba2fdff

renamed service field to priority. Added simple tests

b1089b8

waypoint manager readme

692e5ec

jorgenfj requested review from Andeshog, Q3rkses, kluge7 and philliab13 December 7, 2025 19:09

Andeshog reviewed Dec 7, 2025

View reviewed changes

Andeshog reviewed Dec 8, 2025

View reviewed changes

jorgenfj added 5 commits December 10, 2025 15:17

update with new action def

99def81

removed failing build type

a2f413a

test dependencies

5826046

ignore failing yasmin package

4169c98

remove __init__ files

3afe906

Q3rkses reviewed Dec 11, 2025

View reviewed changes

jorgenfj added 3 commits December 15, 2025 12:22

quat_to_euler in make_pose helper

63f8854

added __init__ file

80e8ce5

removed sim deps for test packages

c65d013

jorgenfj added 4 commits December 18, 2025 12:37

added action shutdown handling

76f49ee

removed waypoint manager set setup

2d27647

added waypoint manager node tests

c0e1185

waypoint manager 4 corner sim test

3a69dbb

jorgenfj added 3 commits December 18, 2025 13:15

added missing launch testing test dependency

546f836

add sleep for topic discovery

1887321

fix action member field name

b26adc7

jorgenfj requested a review from Andeshog December 18, 2025 16:37

Andeshog reviewed Dec 19, 2025

View reviewed changes

removed unnecessary if here

f1f181a

Andeshog approved these changes Dec 19, 2025

View reviewed changes

jorgenfj merged commit 007bb81 into main Dec 19, 2025
14 checks passed

jorgenfj deleted the feat/waypoint_manager branch December 19, 2025 10:48

github-actions Bot added the released label Feb 18, 2026

		Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal);


		if(BUILD_TESTING)

Conversation

jorgenfj commented Dec 7, 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!

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!

chrstrom Dec 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

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!

Q3rkses 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!

jorgenfj commented Dec 17, 2025

Uh oh!

Andeshog commented Dec 17, 2025

Uh oh!

codecov Bot commented Dec 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andeshog left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

github-actions Bot commented Feb 18, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

chrstrom Dec 18, 2025 •

edited

Loading

codecov Bot commented Dec 18, 2025 •

edited

Loading