Feat/waypoint manager #645

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

mission/waypoint_manager/src/waypoint_manager_ros.cpp

+        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

mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp

+    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

mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp

+
+    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

mission/waypoint_manager/src/waypoint_manager_ros.cpp

+
+    (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

guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp

+    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

guidance/reference_filter_dp/src/reference_filter_ros.cpp

+        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

guidance/reference_filter_dp/src/reference_filter_ros.cpp

+        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

mission/waypoint_manager/src/waypoint_manager_ros.cpp

+    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

mission/waypoint_manager/src/waypoint_manager_ros.cpp

+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)

Andeshog · 2025-12-09T17:53:44Z

mission/waypoint_manager/src/waypoint_manager_ros.cpp

+        [this](auto goal_id, auto goal) {
+            return handle_waypoint_goal(goal_id, goal);
+        },
+
+        [this](auto goal_id) { return handle_waypoint_cancel(goal_id); },
+
+        [this](auto goal_handle) {
+            return handle_waypoint_accepted(goal_handle);


Could parameters here be const ref? (too lazy to find out myself)

Andeshog · 2025-12-09T18:08:52Z

tests/simulator_tests/waypoint_manager_test/test/test_four_corner.py

Could this be done for the bash script tests as well? I believe the launch test and script test should coexist, but in my opinion they should do different things. As it is now, the launch tests can directly replace the bash script based node tests, and test that the content in each package works by itself. This also gives test coverage, which is good. The integration tests should then be handled (as they are now) by the github actions workflow, where the whole system is started up in the same way as on deployment. One pro here is that the rosbag (in case of a failing run) is easily accessable from the artifact, as well as nice logs. The potential of using scripting to orchestrate integration tests is also far bigger.

IMO this gets the best of both worlds.

The immediate action I would do is to remove the node tests and put the launch-based node tests in their corresponding package (make an issue on this if you dont want to cook it up now).

Q3rkses

lazy review

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

guidance/reference_filter_dp/src/reference_filter_ros.cpp

+        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

tests/simulator_tests/waypoint_manager_test/waypoint_manager_test/test_common/utils.py

+    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 1 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

guidance/reference_filter_dp/src/reference_filter_ros.cpp

+    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

mission/waypoint_manager/test/CMakeLists.txt

+
+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

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

Andeshog reviewed Dec 9, 2025

View reviewed changes

jorgenfj added 6 commits December 10, 2025 13:59

update util func name

d1557c8

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

mission/waypoint_manager/test/CMakeLists.txt Outdated

Comment on lines 6 to 7

if(BUILD_TESTING)

Copy link

Contributor

Andeshog Dec 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

unnecessary if here?

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

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

Feat/waypoint manager #645

Feat/waypoint manager #645

Uh oh!

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!

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!

Reviewers

Assignees

chrstrom Dec 18, 2025 •

edited

Loading

codecov bot commented Dec 18, 2025 •

edited

Loading