feat(opennav_docking): add on-demand detector client & unit tests for dock plugins (#5015) by bkoensgen · Pull Request #5218 · ros-navigation/navigation2

bkoensgen · 2025-06-02T01:18:21Z

Add detector client initialization in SimpleNonChargingDock
Mirror the same logic in SimpleChargingDock
Expand unit-test coverage for both dock plugins

Basic Info

Info	Value
Ticket(s) this addresses	#5015
Primary OS tested on	Official Nav2 Rolling Docker image
Robotic platform tested on	Unit tests only (`colcon test`)
Does this PR contain AI-generated software?	No

Description of contribution in a few bullet points

Introduce on-demand external-detector control for both charging and non-charging dock plugins
- New parameters: detector_service_name, detector_service_timeout, subscribe_toggle
- Detector started/stopped via std_srvs/Trigger only when first needed
SimpleNonChargingDock now creates the detector client (symmetry with charging dock)
Unit tests added to validate service call logic and TF handling

Description of documentation updates required

Added the three new parameters to nav2_docking/README.md

Description of how this change was tested

colcon test --packages-select opennav_docking → all tests pass
Manual sanity check: plugins load without warnings inside the Rolling dev-container

For Maintainers

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins are added to the plugins page
If BT Node, additionally: add to BT's XML index of nodes for Groot, BT package's readme table, and BT library lists

mergify · 2025-06-02T01:19:37Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski

Good first stab! I have some questions and suggestions, but largely its a good starting point for a couple of hopefully quick iterations. The changes requested in this plugin are the same in both, but I only reviewed the one just to avoid duplicating every comment. Take each comment for both plugin files

nav2_docking/opennav_docking/CMakeLists.txt

nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

codecov · 2025-06-02T16:42:09Z

Codecov Report

❌ Patch coverage is 92.90780% with 10 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...cking/opennav_docking/src/simple_charging_dock.cpp	94.02%	4 Missing ⚠️
...g/opennav_docking/src/simple_non_charging_dock.cpp	94.02%	4 Missing ⚠️
...av2_docking/opennav_docking/src/docking_server.cpp	71.42%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...g/include/opennav_docking/simple_charging_dock.hpp	`100.00% <ø> (ø)`
...clude/opennav_docking/simple_non_charging_dock.hpp	`100.00% <ø> (ø)`
...ore/include/opennav_docking_core/charging_dock.hpp	`80.00% <ø> (ø)`
...include/opennav_docking_core/non_charging_dock.hpp	`87.50% <ø> (ø)`
...av2_docking/opennav_docking/src/docking_server.cpp	`90.40% <71.42%> (+0.35%)`	⬆️
...cking/opennav_docking/src/simple_charging_dock.cpp	`96.05% <94.02%> (-2.17%)`	⬇️
...g/opennav_docking/src/simple_non_charging_dock.cpp	`95.73% <94.02%> (-2.27%)`	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

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

mergify · 2025-06-04T21:05:43Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-04T21:18:30Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2025-06-04T21:45:14Z

@bkoensgen re-request a review from me in the github UX when you're ready for me to take a look again (and CI is building 😉 )

mergify · 2025-06-04T21:45:15Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-04T22:21:20Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-04T23:12:53Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-04T23:26:55Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-04T23:30:21Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T11:52:12Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T13:57:18Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T14:06:59Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T14:18:28Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T14:58:05Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T15:13:18Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T15:38:07Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-05T15:56:23Z

@bkoensgen, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

…d explicit detection state flags Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…ed_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…arted_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com>

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…state Signed-off-by: bkoensgen <b.koensgen@gmail.com>

… behavior (default=false) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…stay in-scope) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…Process() in terminal cleanup Signed-off-by: bkoensgen <b.koensgen@gmail.com>

…ty->stopDetection->terminate) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

bkoensgen · 2025-09-16T20:20:37Z

So MergyBot should be happy now, and I’ve created the PR. Here’s the link: ros-navigation/docs.nav2.org#773

mini-1235 · 2025-09-17T06:23:28Z

nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp

  rclcpp::Rate r(2);
  r.sleep();
-  executor.spin_some();
+  rclcpp::spin_some(node->get_node_base_interface());


Hi, rclcpp::spin_some is going to be deprecated in the next release of rclcpp, this should not be changed, see more information in #5521

@bkoensgen can you address this in a follow up PR?

mini-1235 · 2025-09-17T06:25:31Z

nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp

  rclcpp::Rate r(2);
  r.sleep();
-  executor.spin_some();
+  rclcpp::spin_some(node->get_node_base_interface());


@bkoensgen can you address this in a follow up PR?

mini-1235 · 2025-09-17T06:26:54Z

This PR actually reverts some of the changes in #5521, I think we need to revert the changes related to rclcpp::spin_some

… dock plugins (ros-navigation#5015) (ros-navigation#5218) * feat(opennav_docking): Add dynamic lifecycle for external detectors This change introduces an API to dynamically enable and disable external docking detectors, such as those based on AI, to conserve system resources when not actively docking. Key changes: - Added `startDetectionProcess()` and `stopDetectionProcess()` to the docking plugin interface (`ChargingDock` and `NonChargingDock`). - Implemented this logic in `SimpleChargingDock` and `SimpleNonChargingDock` using a `std_srvs/Trigger` service call and dynamic topic subscription to manage the detector's lifecycle. - The `DockingServer` now ensures `stopDetectionProcess()` is called on all exit paths, including success, failure, and cancellation, to prevent dangling resources. - Added new parameters (`detector_service_name`, `subscribe_toggle`, etc.) to configure this behavior and updated the README documentation. - Added comprehensive C++ unit tests to verify the new detector lifecycle logic, specifically covering service failure cases and proper cleanup on action termination. Closes ros-navigation#5015 Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> - updtae Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix: Address review feedback and fix unit tests Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> fix(docking): Change subscribe_toggle default to false Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> - Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(docking): Improve plugin lifecycle and resource management This commit addresses several code review suggestions to improve the design and robustness of the docking plugins. - Replaced the `DetectorState` enum with a simpler `bool detector_enabled_` for clarity. - Encapsulated the detection process lifecycle within the plugin itself. The `deactivate` method now ensures `stopDetectionProcess` is called, improving encapsulation and simplifying the `DockDatabase`. - Refactored the `configure` method in plugins to group related logic, improving readability. - Cleaned up redundant checks and calls in both the plugins and the `DockingServer` for more robust and intentional code. Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(plugins): Resolve race condition by setting enabled state in callback The previous refactoring introduced a race condition where the detector was marked as 'enabled' in startDetection() before a message was actually received. This caused tests to fail because getRefinedPose() would be called with an enabled state but no valid/recent data. This commit fixes the issue by moving the `detector_enabled_ = true` assignment into the subscription callback. This ensures the plugin's state accurately reflects that it has received at least one valid data point before being considered active. Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(docking): migrate to nav2_ros_common (node_utils, LifecycleNode, QoS) Signed-off-by: bkoensgen <b.koensgen@gmail.com> * build(opennav_docking): update deps (std_srvs, package.xml + CMakeLists) Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(Docking): migrate to nav2::LifecycleNode Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(docking): use nav2::qos::StandardTopicQoS for subscriptions Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(opennav_docking): replace raw queue size with rclcpp::QoS(1) in pubs/subs Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(tests): migrate nav2_util::NodeThread to nav2::NodeThread Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(tests): migrate to 3-args service callbacks Signed-off-by: bkoensgen <b.koensgen@gmail.com> * style(test): apply ament_uncrustify Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(opennav_docking) migrate detector Trigger service to node_->create_client() Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docking: use nav2 types for pubs/subs in SimpleChargingDock and add explicit detection state flags Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docking: use nav2 types for pubs/subs in SimpleNonChargingDock and add explicit detection state flags Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docking: split detection state in SimpleChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docking: split detection state in SimpleNonChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com> * tests: adapt SimpleChargingDockTestable to initial_pose_received_ state Signed-off-by: bkoensgen <b.koensgen@gmail.com> * tests: adapt SimpleNonChargingDockTestable to initial_pose_received_ state Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docs(docking): clarify external detection gating and subscribe_toggle behavior (default=false) Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking): keep SimpleNonChargingDock registered as ChargingDock (stay in-scope) Signed-off-by: bkoensgen <b.koensgen@gmail.com> * docs(docking): revert README note to pre-e881de19 state Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking_server): remove redundant null-check before stopDetectionProcess() in terminal cleanup Signed-off-by: bkoensgen <b.koensgen@gmail.com> * style(docking_server): unify terminal order (stash->publishZeroVelocity->stopDetection->terminate) Signed-off-by: bkoensgen <b.koensgen@gmail.com> * lint Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking): inline detection process Signed-off-by: bkoensgen <b.koensgen@gmail.com> * chore(docking): drop redundant detector comment Signed-off-by: bkoensgen <b.koensgen@gmail.com> * chore(docking): clarify detector logs Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking): activate lifecycle publishers Signed-off-by: bkoensgen <b.koensgen@gmail.com> * chore(docking): reuse dock pose subscription Signed-off-by: bkoensgen <b.koensgen@gmail.com> * lint Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking_server): drop redundant DB deactivate on cleanup Signed-off-by: bkoensgen <b.koensgen@gmail.com> * refactor(docking): rename detection state flag to detection_active Signed-off-by: bkoensgen <b.koensgen@gmail.com> * fix(docking): reset detection flags on cleanup Signed-off-by: bkoensgen <b.koensgen@gmail.com> --------- Signed-off-by: Koensgen Benjamin <b.koensgen@gmail.com> Signed-off-by: bkoensgen <b.koensgen@gmail.com>

bkoensgen force-pushed the feature/detector-toggle-5015 branch from f580de2 to feb37e8 Compare June 2, 2025 14:56

SteveMacenski linked an issue Jun 2, 2025 that may be closed by this pull request

[Docking] Create API to enable / disable detector #5015

Closed

SteveMacenski requested changes Jun 2, 2025

View reviewed changes

bkoensgen marked this pull request as draft June 4, 2025 21:10

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 95ed998 to dcb62d4 Compare June 4, 2025 21:15

bkoensgen force-pushed the feature/detector-toggle-5015 branch from b6ee1e3 to e000987 Compare June 4, 2025 23:27

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 118fe82 to 804520a Compare June 5, 2025 13:54

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 694feb0 to 7153b31 Compare June 5, 2025 14:02

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 49cd52b to 76b096b Compare June 5, 2025 14:15

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 8b7d652 to b3f4910 Compare June 5, 2025 14:54

bkoensgen force-pushed the feature/detector-toggle-5015 branch from cbf5362 to e21641e Compare June 5, 2025 15:34

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 43d42bc to 4e7de42 Compare June 5, 2025 15:52

bkoensgen added 20 commits September 16, 2025 21:03

docking: use nav2 types for pubs/subs in SimpleNonChargingDock and ad…

f1c7634

…d explicit detection state flags Signed-off-by: bkoensgen <b.koensgen@gmail.com>

docking: split detection state in SimpleChargingDock (detection_start…

f0ca065

…ed_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com>

docking: split detection state in SimpleNonChargingDock (detection_st…

a10f89b

…arted_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen <b.koensgen@gmail.com>

tests: adapt SimpleChargingDockTestable to initial_pose_received_ state

9a6c905

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

tests: adapt SimpleNonChargingDockTestable to initial_pose_received_ …

604ea00

…state Signed-off-by: bkoensgen <b.koensgen@gmail.com>

docs(docking): clarify external detection gating and subscribe_toggle…

6e6de33

… behavior (default=false) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking): keep SimpleNonChargingDock registered as ChargingDock (…

816ab57

…stay in-scope) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

docs(docking): revert README note to pre-e881de19 state

a62e800

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking_server): remove redundant null-check before stopDetection…

1294cdf

…Process() in terminal cleanup Signed-off-by: bkoensgen <b.koensgen@gmail.com>

style(docking_server): unify terminal order (stash->publishZeroVeloci…

a3f4f37

…ty->stopDetection->terminate) Signed-off-by: bkoensgen <b.koensgen@gmail.com>

lint

b2fa664

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking): inline detection process

5500dfa

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

chore(docking): drop redundant detector comment

7edfb0d

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

chore(docking): clarify detector logs

5102f5d

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking): activate lifecycle publishers

79537f8

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

chore(docking): reuse dock pose subscription

67a8079

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

lint

82e7712

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking_server): drop redundant DB deactivate on cleanup

16412c8

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

refactor(docking): rename detection state flag to detection_active

4783ff2

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

fix(docking): reset detection flags on cleanup

45f4bcb

Signed-off-by: bkoensgen <b.koensgen@gmail.com>

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 9062ad0 to 45f4bcb Compare September 16, 2025 19:07

SteveMacenski approved these changes Sep 16, 2025

View reviewed changes

SteveMacenski merged commit 4d05d23 into ros-navigation:main Sep 16, 2025
16 checks passed

mini-1235 reviewed Sep 17, 2025

View reviewed changes

bkoensgen mentioned this pull request Sep 18, 2025

restore executor-based spinning in opennav_docking #5533

Merged

Conversation

bkoensgen commented Jun 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required

Description of how this change was tested

For Maintainers

Uh oh!

mergify bot commented Jun 2, 2025

Uh oh!

SteveMacenski left a comment • 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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Jun 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

SteveMacenski commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 4, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

mergify bot commented Jun 5, 2025

Uh oh!

bkoensgen commented Sep 16, 2025

Uh oh!

Uh oh!

mini-1235 Sep 17, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Sep 17, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 Sep 17, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Sep 17, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 commented Sep 17, 2025

Uh oh!

Reviewers

Assignees

bkoensgen commented Jun 2, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

codecov bot commented Jun 2, 2025 •

edited

Loading