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

nav2_docking/README.md

@@ -36,12 +36,14 @@ The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizab
 The docking procedure is as follows:
 1. Take action request and obtain the dock's plugin and its pose
 2. If the robot is not within the prestaging tolerance of the dock's staging pose, navigate to the staging pose
-3. Use the dock's plugin to initially detect the dock and return the docking pose
-4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system
-5. Exit the vision-control loop once contact has been detected or charging has started
-6. Wait until charging starts (if applicable) and return success.
+3. Call the dock's plugin `startDetectionProcess()` method to activate any external detection mechanisms.
+4. Use the dock's plugin to initially detect the dock (`getRefinedPose`) and return the docking pose.
+5. Enter a vision-control loop where the robot attempts to reach the docking pose while it's actively being refined by the vision system.
+6. Exit the vision-control loop once contact has been detected or charging has started (if applicable).
+7. Wait until charging starts (if applicable) and return success.
+8. Call the dock's plugin `stopDetectionProcess()` method to deactivate any external detection mechanisms.
 
-If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client.
+If anywhere this procedure is unsuccessful (before step 8), `N` retries may be made, driving back to the dock's staging pose, and then restarting the process from step 3. If still unsuccessful after retries, it will return a failure code to indicate what kind of failure occurred to the client.
 
 Undocking works more simply:
 1. If previously docked, use the known dock information to get the dock type. If not, use the undock action request's indicated dock type
@@ -175,8 +177,9 @@ some robots.
 `getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose.
 
 `getRefinedPose` can be used in two ways.
-1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation.
+1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. The `start/stopDetectionProcess` methods would typically do nothing in this case.
 2. The more realistic use case is to use an AR marker, dock pose detection algorithm, etc. The plugin will subscribe to a `geometry_msgs/PoseStamped` topic `detected_dock_pose`. This can be used with the `image_proc/TrackMarkerNode` for Apriltags or other custom detectors for your dock. It is unlikely the detected pose is actually the pose you want to dock with, so several parameters are supplied to represent your docked pose relative to the detected feature's pose.
+The subscription to `detected_dock_pose` can be managed dynamically (see `subscribe_toggle` parameter). Additionally, an external detector process can be triggered via a service call (see `detector_service_name`). The `DockingServer` calls `startDetectionProcess()` before `getRefinedPose` (which is called in a loop) and `stopDetectionProcess()` after the docking action concludes (successfully or not). This allows plugins to manage resources like GPU usage by only running detection when needed.
 
 During the docking approach, there are two options for detecting `isDocked`:
 1. We can check the joint states of the wheels if the current has spiked above a set threshold to indicate that the robot has made contact with the dock or other physical object.
@@ -257,6 +260,9 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required.
 | staging_yaw_offset        | Staging pose angle relative to dock pose (rad)    | double |  0.0    |
 | dock_direction        | Whether the robot is docking with the dock forward or backward in motion | string |  "forward"  or "backward"    |
 | rotate_to_dock        | Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. | bool |  false      |
+| detector_service_name      | Trigger service name to start/stop the detector (e.g., a camera node or an AprilTag detector node). Leave empty to disable service calls. | string | ""    |
+| detector_service_timeout   | Timeout (seconds) to wait for the detector service (if `detector_service_name` is set) to become available or respond. | double | 5.0   |
+| subscribe_toggle           | If true, dynamically subscribe/unsubscribe to `detected_dock_pose` topic when `start/stopDetectionProcess` are called. If false, the subscription is kept alive throughout the plugin's lifecycle if `use_external_detection_pose` is true. This can be useful if the detector node is always running and publishing. | bool   | false  |
 
 Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`.
 

nav2_docking/opennav_docking/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -127,6 +128,7 @@ target_link_libraries(simple_charging_dock PUBLIC
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
   nav2_ros_common::nav2_ros_common
@@ -155,6 +157,7 @@ target_link_libraries(simple_non_charging_dock PUBLIC
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
 )
@@ -211,6 +214,7 @@ ament_export_dependencies(
   rclcpp_action
   rclcpp_lifecycle
   rcl_interfaces
+  std_srvs
   tf2_ros
   yaml-cpp
 )

nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <vector>
 
+#include "std_srvs/srv/trigger.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -54,17 +55,17 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
   /**
    * @brief Method to cleanup resources used on shutdown.
    */
-  virtual void cleanup() {}
+  void cleanup() override;
 
   /**
    * @brief Method to active Behavior and any threads involved in execution.
    */
-  virtual void activate() {}
+  void activate() override;
 
   /**
    * @brief Method to deactivate Behavior and any threads involved in execution.
    */
-  virtual void deactivate() {}
+  void deactivate() override;
 
   /**
    * @brief Method to obtain the dock's staging pose. This method should likely
@@ -104,14 +105,24 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
    */
   virtual bool hasStoppedCharging();
 
+  /**
+   * @brief Start external detection process (service call + subscribe).
+   */
+  bool startDetectionProcess() override;
+
+  /**
+   * @brief Stop external detection process.
+   */
+  bool stopDetectionProcess() override;
+
 protected:
   void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
 
   // Optionally subscribe to a detected dock pose topic
   nav2::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr filtered_dock_pose_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr staging_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr filtered_dock_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr staging_pose_pub_;
   // If subscribed to a detected pose topic, will contain latest message
   geometry_msgs::msg::PoseStamped detected_dock_pose_;
   // This is the actual dock pose once it has the specified translation/rotation applied
@@ -150,6 +161,18 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
 
   nav2::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+
+  // Detector control parameters
+  std::string detector_service_name_;
+  double detector_service_timeout_{5.0};
+  bool subscribe_toggle_{false};
+
+  // Client used to call the Trigger service
+  nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr detector_client_;
+
+  // Detection state flags
+  bool detection_active_{false};
+  bool initial_pose_received_{false};
 };
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <vector>
 
+#include "std_srvs/srv/trigger.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -54,17 +55,17 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
   /**
    * @brief Method to cleanup resources used on shutdown.
    */
-  virtual void cleanup() {}
+  void cleanup() override;
 
-  /**
-   * @brief Method to active Behavior and any threads involved in execution.
-   */
-  virtual void activate() {}
+   /**
+    * @brief Method to active Behavior and any threads involved in execution.
+    */
+  void activate() override;
 
-  /**
-   * @brief Method to deactivate Behavior and any threads involved in execution.
-   */
-  virtual void deactivate() {}
+   /**
+    * @brief Method to deactivate Behavior and any threads involved in execution.
+    */
+  void deactivate() override;
 
   /**
    * @brief Method to obtain the dock's staging pose. This method should likely
@@ -89,14 +90,24 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
    */
   virtual bool isDocked();
 
+  /**
+   * @brief Start external detection process (service call + subscribe).
+   */
+  bool startDetectionProcess() override;
+
+  /**
+   * @brief Stop external detection process.
+   */
+  bool stopDetectionProcess() override;
+
 protected:
   void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
 
   // Optionally subscribe to a detected dock pose topic
   nav2::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr filtered_dock_pose_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr staging_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr filtered_dock_pose_pub_;
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr staging_pose_pub_;
   // If subscribed to a detected pose topic, will contain latest message
   geometry_msgs::msg::PoseStamped detected_dock_pose_;
   // This is the actual dock pose once it has the specified translation/rotation applied
@@ -128,6 +139,18 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
 
   nav2::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+
+  // Detector control parameters
+  std::string detector_service_name_;
+  double detector_service_timeout_{5.0};
+  bool subscribe_toggle_{false};
+
+  // Client used to call the Trigger service
+  nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr detector_client_;
+
+  // Detection state flags
+  bool detection_active_{false};
+  bool initial_pose_received_{false};
 };
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/package.xml

@@ -25,6 +25,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rcl_interfaces</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -331,6 +331,7 @@ void DockingServer::dockRobot()
             result->num_retries = num_retries_;
             stashDockData(goal->use_dock_id, dock, true);
             publishZeroVelocity();
+            dock->plugin->stopDetectionProcess();
             docking_action_server_->succeeded_current(result);
             return;
           }
@@ -339,6 +340,7 @@ void DockingServer::dockRobot()
         // Cancelled, preempted, or shutting down (recoverable errors throw DockingException)
         stashDockData(goal->use_dock_id, dock, false);
         publishZeroVelocity();
+        dock->plugin->stopDetectionProcess();
         docking_action_server_->terminate_all(result);
         return;
       } catch (opennav_docking_core::DockingException & e) {
@@ -354,6 +356,7 @@ void DockingServer::dockRobot()
         // Cancelled, preempted, or shutting down
         stashDockData(goal->use_dock_id, dock, false);
         publishZeroVelocity();
+        dock->plugin->stopDetectionProcess();
         docking_action_server_->terminate_all(result);
         return;
       }
@@ -401,6 +404,7 @@ void DockingServer::dockRobot()
   stashDockData(goal->use_dock_id, dock, false);
   result->num_retries = num_retries_;
   publishZeroVelocity();
+  dock->plugin->stopDetectionProcess();
   docking_action_server_->terminate_current(result);
 }
 
@@ -429,12 +433,18 @@ Dock * DockingServer::generateGoalDock(std::shared_ptr<const DockRobot::Goal> go
 void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose)
 {
   publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION);
+
+  if (!dock->plugin->startDetectionProcess()) {
+    throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process.");
+  }
+
   rclcpp::Rate loop_rate(controller_frequency_);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_);
   while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
     if (this->now() - start > timeout) {
-      throw opennav_docking_core::FailedToDetectDock("Failed initial dock detection");
+      throw opennav_docking_core::FailedToDetectDock(
+        "Failed initial dock detection: Timeout exceeded");
     }
 
     if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||