diff --git a/nav2_docking/README.md b/nav2_docking/README.md index f246c0c2493..2d2d92d8f97 100644 --- a/nav2_docking/README.md +++ b/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`. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 8801b18d5a8..cbb068aa504 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/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 ) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 719e6e5cf7d..7cc33d34c4e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#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::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::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_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::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp index a0b3a46531e..ef2c7082e2c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#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::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::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_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::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index b65cb91ee56..97edbafb954 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -25,6 +25,7 @@ rclcpp_lifecycle rcl_interfaces sensor_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 1208ade5521..ec7a7fc5e81 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/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 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(docking_action_server_, "dock_robot") || diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index ff8be965603..3be3428e4eb 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -37,6 +40,14 @@ void SimpleChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".use_battery_status", rclcpp::ParameterValue(true)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Parameters for optional external detection of dock pose nav2::declare_parameter_if_not_declared( node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); @@ -105,6 +116,26 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -123,21 +154,17 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); + } + if (use_battery_status_) { battery_sub_ = node_->create_subscription( "battery_state", [this](const sensor_msgs::msg::BatteryState::SharedPtr state) { is_charging_ = state->current > charging_threshold_; - }, nav2::qos::StandardTopicQoS()); - } - - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent pose + }); } bool use_stall_detection; @@ -197,6 +224,12 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -323,6 +356,120 @@ void SimpleChargingDock::jointStateCallback(const sensor_msgs::msg::JointState:: is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock deactivated"); +} + +void SimpleChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index 2a845559c6d..7e2aa26cf85 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_non_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -50,6 +53,14 @@ void SimpleNonChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Optionally determine if docked via stall detection using joint_states nav2::declare_parameter_if_not_declared( node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); @@ -94,6 +105,26 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -112,13 +143,9 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent one + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); } bool use_stall_detection; @@ -178,6 +205,12 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -289,6 +322,120 @@ void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointStat is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleNonChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleNonChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleNonChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleNonChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock deactivated"); +} + +void SimpleNonChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 8974c236ab0..46d69b0e40c 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -28,6 +28,7 @@ import launch_testing.asserts import launch_testing.markers import launch_testing.util +from lifecycle_msgs.srv import GetState from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot from nav_msgs.msg import Odometry @@ -36,6 +37,7 @@ from rclpy.action.client import ActionClient from rclpy.action.server import ActionServer, ServerGoalHandle from sensor_msgs.msg import BatteryState +import tf2_ros from tf2_ros import TransformBroadcaster # This test can be run standalone with: @@ -127,8 +129,28 @@ def setUp(self) -> None: self.command = Twist() self.node = rclpy.create_node('test_docking_server') # Publish odometry + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) + def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0): + """Wait for a managed node to become active.""" + client = self.node.create_client(GetState, f'{node_name}/get_state') + if not client.wait_for_service(timeout_sec=2.0): + self.fail(f'Service get_state for {node_name} not available.') + + start_time = time.time() + while time.time() - start_time < timeout_sec: + req = GetState.Request() + future = client.call_async(req) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + if future.result() and future.result().current_state.id == 3: # 3 = ACTIVE + self.node.get_logger().info(f'Node {node_name} is active.') + return + time.sleep(0.5) + # raises AssertionError + self.fail(f'Node {node_name} did not become active within {timeout_sec} seconds.') + def tearDown(self) -> None: self.node.destroy_node() @@ -206,6 +228,7 @@ def nav_execute_callback( def test_docking_server(self) -> None: # Publish TF for odometry self.tf_broadcaster = TransformBroadcaster(self.node) + time.sleep(0.5) # Create a timer to run "control loop" at 20hz self.timer = self.node.create_timer(0.05, self.timer_callback) @@ -245,10 +268,19 @@ def test_docking_server(self) -> None: # Publish transform self.publish() - # Run for 1 seconds to allow tf to propagate - for _ in range(10): + # Wait until the transform is available. + self.node.get_logger().info('Waiting for TF odom->base_link to be available...') + start_time = time.time() + timeout = 10.0 + while not self.tf_buffer.can_transform('odom', 'base_link', rclpy.time.Time()): + if time.time() - start_time > timeout: + self.fail('TF transform odom->base_link not available after 10s') rclpy.spin_once(self.node, timeout_sec=0.1) time.sleep(0.1) + self.node.get_logger().info('TF is ready, proceeding with test.') + + # Wait until the docking server is active. + self.wait_for_node_to_be_active('docking_server') # Test docking action self.action_result = [] diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 7cc1f2fe8ab..22d676bf60a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -46,6 +46,15 @@ class DockingServerShim : public DockingServer } }; +// Test shim to expose the TF2 buffer for injection +class DockingServerTFShim : public DockingServerShim +{ +public: + DockingServerTFShim() + : DockingServerShim() {} + std::shared_ptr getTfBuffer() {return tf2_buffer_;} +}; + TEST(DockingServerTests, ObjectLifecycle) { auto node = std::make_shared(); @@ -311,6 +320,161 @@ TEST(DockingServerTests, testDockBackward) node.reset(); } +TEST(DockingServerTests, ExceptionHandlingDuringDocking) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client"); + + // Configure docking server + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + // Test multiple exception scenarios to ensure coverage + std::vector exceptions = {"FailedToDetectDock", "FailedToControl"}; + + for (const auto & exception_type : exceptions) { + node->set_parameter(rclcpp::Parameter("exception_to_throw", exception_type)); + node->set_parameter(rclcpp::Parameter("dock_action_called", false)); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + auto goal = DockRobot::Goal(); + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + auto status = rclcpp::spin_until_future_complete(client_node, future_result, 5s); + ASSERT_EQ(status, rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + } + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); +} + +TEST(DockingServerTests, StopDetectionOnSuccess) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_success"); + + // Configure the server with the test plugin + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + + // Configure TestFailureDock to report success + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", true); + // Note: isCharging() in TestFailureDock returns false, so it will wait for charge + // which will succeed because the plugin is a charger. We'll set the timeout low. + node->set_parameter(rclcpp::Parameter("wait_charge_timeout", 0.1)); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(result.result->success); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + +TEST(DockingServerTests, HandlesPluginStartFailure) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_start_failure"); + + // Configure the server with the TestFailureDock plugin. + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("test_dock.frame", "odom"); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + + // Configure the TestFailureDock to fail its startup process. + node->declare_parameter("fail_start_detection", true); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + + // Mock the necessary TF transform to prevent a premature failure. + geometry_msgs::msg::TransformStamped identity_transform; + identity_transform.header.frame_id = "odom"; + identity_transform.child_frame_id = "odom"; + identity_transform.transform.rotation.w = 1.0; + node->getTfBuffer()->setTransform(identity_transform, "test_authority", true); + + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + EXPECT_EQ(result.result->error_code, DockRobot::Result::FAILED_TO_DETECT_DOCK); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 3b96f59149f..a95bd36eda3 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleChargingDockShim : public opennav_docking::SimpleChargingDock } }; +class SimpleChargingDockTestable : public opennav_docking::SimpleChargingDock +{ +public: + using opennav_docking::SimpleChargingDock::SimpleChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_FALSE(dock->isCharging()); @@ -71,9 +86,6 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -83,7 +95,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); @@ -94,7 +106,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isCharging()); EXPECT_FALSE(dock->hasStoppedCharging()); @@ -127,9 +139,6 @@ TEST(SimpleChargingDockTests, StallDetection) rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - EXPECT_EQ(dock->getStallJointNames(), names); // Stopped, but below effort threshold @@ -140,7 +149,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -152,7 +161,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -164,7 +173,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isDocked()); @@ -235,8 +244,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -250,7 +258,8 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -258,6 +267,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -277,8 +287,6 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -286,7 +294,7 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -316,16 +324,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - executor.spin_some(); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -338,11 +337,24 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -413,6 +425,208 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared("test_subscription_reliable"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared("test_detector_failure", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared("test_sub_persistent", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 82839040c18..77951bd6cd5 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple non-charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleNonChargingDockShim : public opennav_docking::SimpleNonChargingDock } }; +class SimpleNonChargingDockTestable : public opennav_docking::SimpleNonChargingDock +{ +public: + using opennav_docking::SimpleNonChargingDock::SimpleNonChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleNonChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleNonChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_THROW(dock->isCharging(), std::runtime_error); @@ -84,9 +99,6 @@ TEST(SimpleNonChargingDockTests, StallDetection) EXPECT_EQ(dock->getStallJointNames(), names); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -98,7 +110,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -110,7 +122,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -122,7 +134,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isDocked()); @@ -193,8 +205,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -208,7 +219,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -216,6 +228,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -235,8 +248,6 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -244,7 +255,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -274,16 +285,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - executor.spin_some(); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -296,11 +298,24 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -371,6 +386,211 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleNonChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleNonChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared( + "test_subscription_reliable_non_charging"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "non_charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared( + "test_detector_failure_non_charging", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared( + "test_sub_persistent_non_charging", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp index 8a33c3b3304..a5ee4adaff5 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -102,6 +102,18 @@ class TestFailureDock : public opennav_docking_core::ChargingDock return true; } + virtual bool startDetectionProcess() + { + bool should_fail; + node_->get_parameter_or("fail_start_detection", should_fail, false); + return !should_fail; + } + + virtual bool stopDetectionProcess() + { + return true; + } + protected: nav2::LifecycleNode::SharedPtr node_; }; diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index 67e599405d7..111dc1c03ef 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -124,6 +124,16 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */ diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp index 9422ee19f13..97b9dd06e61 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -125,6 +125,16 @@ class NonChargingDock : public ChargingDock throw std::runtime_error("This dock is not a charging dock!"); } + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */