feat(opennav_docking): add on-demand detector client & unit tests for dock plugins (#5015) #5218

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

SteveMacenski · 2025-06-02T16:23:15Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

@@ -133,11 +154,6 @@ void SimpleChargingDock::configure(

  if (use_external_detection_pose_) {
    dock_pose_.header.stamp = rclcpp::Time(0);
-    dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(


If !subscribe_toggle_, I think we should create this, no?

I think the code should move back up here. You also don't need to check !detected_pose_sub_

@bkoensgen this was not completed :-)

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

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

Codecov Report

Attention: Patch coverage is 87.93103% with 14 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...cking/opennav_docking/src/simple_charging_dock.cpp	88.00%	6 Missing ⚠️
...g/opennav_docking/src/simple_non_charging_dock.cpp	88.00%	6 Missing ⚠️
...av2_docking/opennav_docking/src/docking_server.cpp	81.81%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...g/include/opennav_docking/simple_charging_dock.hpp	`100.00% <100.00%> (ø)`
...clude/opennav_docking/simple_non_charging_dock.hpp	`100.00% <100.00%> (ø)`
nav2_docking/opennav_docking/src/dock_database.cpp	`98.76% <100.00%> (+0.01%)`	⬆️
...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.50% <81.81%> (+0.41%)`	⬆️
...cking/opennav_docking/src/simple_charging_dock.cpp	`94.81% <88.00%> (-3.39%)`	⬇️
...g/opennav_docking/src/simple_non_charging_dock.cpp	`94.35% <88.00%> (-3.63%)`	⬇️

... and 8 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).

mergify · 2025-06-13T15:33:35Z

@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-15T17: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).

bkoensgen · 2025-06-16T11:44:28Z

@SteveMacenski

Thanks for the detailed review and guidance! I've pushed up the changes addressing your feedback.

The biggest change is that I've moved the detector lifecycle management (start/stopDetectionProcess) into the plugin API itself. The DockingServer now just calls these methods, which gives individual plugins full control over how and when they manage their perception resources (like starting/stopping a GPU-intensive node). This felt a lot cleaner and more robust.

Here’s a quick rundown of the other changes:

Service Client & Dependencies: Switched over to nav2_util::ServiceClient as you suggested and cleaned up the CMakeLists to properly link std_srvs.
Test Coverage: Beefed up the unit tests for both SimpleChargingDock and SimpleNonChargingDock. They now cover the new subscribe_toggle logic and the service failure edge cases we discussed.
Test Reliability: That also gave me a chance to fix a race condition in the Python smoke tests related to TF propagation. They should be much more stable in CI now.
Git History: Squashed everything down to a single commit for a clean merge.

Should be in good shape for another look now. Let me know what you think!

SteveMacenski

Really good job on the tests, thanks for that! Beyond these changes which are mostly superficial, we need to also have the migration guide and configuration guide for docking server updated for these new parameters and capabilities. Please open a PR to include these & I think this is largely ready to go! :-)

All comments I made in the simple charging dock also apply to the implementation of the non-charging plugin too

nav2_docking/opennav_docking/test/test_docking_server.py

nav2_docking/README.md

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

SteveMacenski · 2025-06-16T19:34:35Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+    detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+      "detected_dock_pose", rclcpp::SensorDataQoS(),
+      [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
+        detected_dock_pose_ = *pose;


Should uses of detected_dock_pose_ make sure that detector_state_ is on? Throw otherwise

This was not completed either

good point. I checked getRefinedPose and it already has a guard clause at the top (if (!detector_enabled_)) that prevents detected_dock_pose_ from being used improperly.

Returning false there seems like the right behavior for the control loop, so I think we're all set on this one.

SteveMacenski · 2025-06-16T19:37:30Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+      "detected_dock_pose", rclcpp::SensorDataQoS(),
+      [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
+        detected_dock_pose_ = *pose;
+        detector_state_ = DetectorState::ON;


Remove. This is always set anyway on L384 below when we exist the function. It has no impact in the callback (ditto for the subscriber above).

This was not completed either

You're right, I did remove this initially based on your feedback! But it turned out to cause a subtle race condition that made the tests fail.

What happened was that startDetection() would set the detector to "enabled" immediately, but getRefinedPose() would then be called before the first message from the topic actually arrived. This caused the timestamp check to fail.

The only way I found to make it robustly pass the tests was to put the state update back here, inside the callback. This guarantees the plugin only considers itself 'active' once it has actually received data.

It's a bit counter-intuitive, but it seems necessary to handle that timing issue. Let me know if you have other ideas on how to tackle this!

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>

nav2_docking/README.md

SteveMacenski · 2025-06-17T20:04:29Z

Mark this as ready to review again once you'd like me to take a look!

SteveMacenski · 2025-06-17T20:20:58Z

nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

+  // Detector control parameters
+  std::string detector_service_name_;
+  double detector_service_timeout_{5.0};
+  bool subscribe_toggle_{true};


Suggested change

bool subscribe_toggle_{true};

bool subscribe_toggle_{false};

SteveMacenski · 2025-06-17T20:21:05Z

nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp

+  // Detector control parameters
+  std::string detector_service_name_;
+  double detector_service_timeout_{5.0};
+  bool subscribe_toggle_{true};


Suggested change

bool subscribe_toggle_{true};

bool subscribe_toggle_{false};

SteveMacenski · 2025-06-17T20:21:40Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

@@ -133,11 +154,6 @@ void SimpleChargingDock::configure(

  if (use_external_detection_pose_) {
    dock_pose_.header.stamp = rclcpp::Time(0);
-    dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(


@bkoensgen this was not completed :-)

SteveMacenski · 2025-06-17T20:22:39Z

nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

+  /**
+   * @brief Start external detection process (service call + subscribe).
+   */
+  void startDetectionProcess() override {startDetection();}


@bkoensgen this was not completed :-)

SteveMacenski · 2025-06-17T20:22:44Z

nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr detected_pose_sub_;
+
+  // Simple finite-state machine for detector status
+  enum class DetectorState { OFF, ON };


@bkoensgen this was not completed :-)

SteveMacenski · 2025-06-17T20:23:20Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

@@ -133,11 +155,6 @@ void SimpleChargingDock::configure(

  if (use_external_detection_pose_) {


This was not completed either

SteveMacenski · 2025-06-17T20:23:31Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+
+void SimpleChargingDock::deactivate()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock deactivated");


This was not completed either

SteveMacenski · 2025-06-17T20:23:33Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+
+void SimpleChargingDock::activate()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock activated");


This was not completed either

SteveMacenski · 2025-06-17T20:23:51Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+    detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+      "detected_dock_pose", rclcpp::SensorDataQoS(),
+      [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
+        detected_dock_pose_ = *pose;


This was not completed either

SteveMacenski · 2025-06-17T20:23:54Z

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+      "detected_dock_pose", rclcpp::SensorDataQoS(),
+      [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
+        detected_dock_pose_ = *pose;
+        detector_state_ = DetectorState::ON;


This was not completed either

SteveMacenski · 2025-06-17T20:24:22Z

@bkoensgen the majority of my review comments were not completed, please see above

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

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>

…back 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>

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

Open

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 force-pushed the feature/detector-toggle-5015 branch from ccd84ce to 4b0b1da Compare June 13, 2025 14:46

bkoensgen force-pushed the feature/detector-toggle-5015 branch 2 times, most recently from 9750cac to 5bd06d3 Compare June 15, 2025 17:00

bkoensgen force-pushed the feature/detector-toggle-5015 branch 7 times, most recently from dc2325d to 7c5c042 Compare June 16, 2025 10:44

bkoensgen requested a review from SteveMacenski June 16, 2025 11:44

SteveMacenski reviewed Jun 16, 2025

View reviewed changes

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 896fa48 to d88d2df Compare June 17, 2025 14:25

SteveMacenski reviewed Jun 17, 2025

View reviewed changes

nav2_docking/README.md Outdated Show resolved Hide resolved

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 15caec9 to 2e6dc99 Compare June 17, 2025 19:14

bkoensgen requested a review from SteveMacenski June 17, 2025 20:18

SteveMacenski reviewed Jun 17, 2025

View reviewed changes

fix: Address review feedback and fix unit tests

2f12cf1

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

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 2e6dc99 to 2f12cf1 Compare June 17, 2025 20:50

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 74b8455 to fc7ae9f Compare June 18, 2025 14:05

bkoensgen force-pushed the feature/detector-toggle-5015 branch from 3fb99d3 to 96bdb23 Compare June 18, 2025 15:16

bkoensgen requested a review from SteveMacenski June 18, 2025 16:05

		@@ -133,11 +155,6 @@ void SimpleChargingDock::configure(

		if (use_external_detection_pose_) {

feat(opennav_docking): add on-demand detector client & unit tests for dock plugins (#5015) #5218

Are you sure you want to change the base?

feat(opennav_docking): add on-demand detector client & unit tests for dock plugins (#5015) #5218

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!

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!

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!

mergify bot commented Jun 13, 2025

Uh oh!

mergify bot commented Jun 15, 2025

Uh oh!

bkoensgen commented Jun 16, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

bkoensgen commented Jun 2, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

codecov bot commented Jun 2, 2025 •

edited

Loading