diff --git a/nav2_docking/README.md b/nav2_docking/README.md index c1aa1b10da..fba8ca1139 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -1,6 +1,6 @@ # Open Navigation's Nav2 Docking Framework -This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). +This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, non-charging dock request needs, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). This work is sponsored by [NVIDIA](https://www.nvidia.com/en-us/) and created by [Open Navigation LLC](https://opennav.org). @@ -24,9 +24,9 @@ The Docking Framework has 5 main components: - `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances - `DockDatabase`: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported. - `Controller`: A spiral-based graceful controller to use for the vision-control loop for docking -- `ChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find this plugin header in the `opennav_docking_core` package. +- `ChargingDock` and `NonChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find these plugin headers in the `opennav_docking_core` package. -The `ChargingDock` plugins are the heart of the customizability of the framework to support any type of charging dock for any kind of robot. The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. +The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizability of the framework to support any type of charging or non-charging dock for any kind of robot. This means you can both dock with charging stations, as well as non-charging infrastructure such as static locations (ex. conveyers) or dynamic locations (ex. pallets). The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. For dynamic locations like docking with movable objects, the action request can include an approximate docking location that uses vision-control loops to refine motion into it. The docking procedure is as follows: 1. Take action request and obtain the dock's plugin and its pose @@ -34,7 +34,7 @@ The docking procedure is as follows: 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 and return success. +6. Wait until charging starts (if applicable) and return success. 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. @@ -74,7 +74,7 @@ This service exists to potentially reload the dock server's known dock database There are two unique elements to consider in specifying docks: dock _instances_ and dock _plugins_. Dock instances are instances of a particular dock in the map, as the database may contain many known docks (and you can specify which by name you'd like to dock at). Dock plugins are the model of dock that each is an instance of. The plugins contain the capabilities to generically detect and connect to a particular dock model. This separation allows us to efficiently enable many individual docking locations of potentially several different revisions with different attributes. -The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). +The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). These can be of both charging and non-charging types. ``` dock_plugins: ["dockv1", "dockv3"] @@ -145,23 +145,26 @@ There are two functions used during dock approach: * `isCharging`: The approach stops when the robot reports `isDocked`, then we wait for charging to start by calling `isCharging`. * This may be implemented using the `sensor_msgs/BatteryState` message to check the power status or for charging current. + * Used for charging-typed plugins. Similarly, there are two functions used during undocking: * `disableCharging`: This function is called before undocking commences to help prevent wear on the charge contacts. If the charge dock supports turning off the charge current, it should be done here. + * Used for charging-typed plugins. * `hasStoppedCharging`: This function is called while the controller is undocking. Undocking is successful when charging has stopped and the robot has returned to the staging pose. + * Used for charging-typed plugins. Keep in mind that the docking and undocking functions should return quickly as they will be called inside the control loop. Also make sure that `isDocked` should return true if `isCharging` returns true. -### Simple Charging Dock Plugin +### Simple (Non-) Charging Dock Plugins -The `SimpleChargingDock` plugin is an example with many common options which may be fully functional for +The `SimpleChargingDock` and `SimpleNonChargingDock` plugins are examples with many common options which may be fully functional for some robots. `getStagingPose` applys a parameterized translational and rotational offset to the dock pose to obtain the staging pose. @@ -174,7 +177,7 @@ 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. 2. The dock pose is compared with the robot pose and `isDocked` returns true when the distance drops below the specified `docking_threshold`. -The `isCharging` and `hasStoppedCharging` functions have two options: +The `isCharging` and `hasStoppedCharging` functions have two options, when using the charging dock type: 1. Subscribing to a `sensor_msgs/BatteryState` message on topic `battery_state`. The robot is considered charging when the `current` field of the message exceeds the `charging_threshold`. 2. We can return that we are charging is `isDocked() = true`, which is useful for initial testing or low-reliability docking until battery state or similar information is available. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index d991a25ba9..add25f4754 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -126,9 +126,33 @@ target_link_libraries(simple_charging_dock PRIVATE tf2::tf2 ) +add_library(simple_non_charging_dock SHARED + src/simple_non_charging_dock.cpp +) +target_include_directories(simple_non_charging_dock + PUBLIC + "$" + "$" +) +target_link_libraries(simple_non_charging_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(simple_non_charging_dock PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 +) + pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) -install(TARGETS ${library_name} controller pose_filter simple_charging_dock +install(TARGETS ${library_name} controller pose_filter simple_charging_dock simple_non_charging_dock EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib 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 2b877442f5..49adf4124c 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 @@ -142,6 +142,7 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock double charging_threshold_; // If not using an external pose reference, this is the distance threshold double docking_threshold_; + std::string base_frame_id_; // Offset for staging pose relative to dock pose double staging_x_offset_; double staging_yaw_offset_; 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 new file mode 100644 index 0000000000..0f25828944 --- /dev/null +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +#include "opennav_docking_core/non_charging_dock.hpp" +#include "opennav_docking/pose_filter.hpp" + +namespace opennav_docking +{ + +class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock +{ +public: + /** + * @brief Constructor + */ + SimpleNonChargingDock() + : NonChargingDock() + {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf); + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() {} + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() {} + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() {} + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock with pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame); + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id); + + /** + * @copydoc opennav_docking_core::ChargingDock::isDocked + */ + virtual bool isDocked(); + +protected: + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); + + // Optionally subscribe to a detected dock pose topic + rclcpp::Subscription::SharedPtr dock_pose_sub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; + rclcpp::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 + // If not subscribed to a topic, this is simply the database dock pose + geometry_msgs::msg::PoseStamped dock_pose_; + + // Optionally subscribe to joint state message, used to determine if stalled + rclcpp::Subscription::SharedPtr joint_state_sub_; + std::vector stall_joint_names_; + double stall_velocity_threshold_, stall_effort_threshold_; + bool is_stalled_; + + // An external reference (such as image_proc::TrackMarkerNode) can be used to detect dock + bool use_external_detection_pose_; + double external_detection_timeout_; + tf2::Quaternion external_detection_rotation_; + double external_detection_translation_x_; + double external_detection_translation_y_; + + // Filtering of detected poses + std::shared_ptr filter_; + + // If not using an external pose reference, this is the distance threshold + double docking_threshold_; + std::string base_frame_id_; + // Offset for staging pose relative to dock pose + double staging_x_offset_; + double staging_yaw_offset_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr tf2_buffer_; +}; + +} // namespace opennav_docking + +#endif // OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ diff --git a/nav2_docking/opennav_docking/plugins.xml b/nav2_docking/opennav_docking/plugins.xml index 2f1da5f283..3a3c91f067 100644 --- a/nav2_docking/opennav_docking/plugins.xml +++ b/nav2_docking/opennav_docking/plugins.xml @@ -4,6 +4,11 @@ A simple charging dock plugin. + + + A simple non-charging dock plugin. + + diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index a8bf62a6b2..0d5034bd60 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -227,19 +227,20 @@ void DockingServer::dockRobot() if (goal->use_dock_id) { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger %s.", goal->dock_id.c_str()); + "Attempting to dock robot at %s.", goal->dock_id.c_str()); dock = dock_db_->findDock(goal->dock_id); } else { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger at position (%0.2f, %0.2f).", + "Attempting to dock robot at position (%0.2f, %0.2f).", goal->dock_pose.pose.position.x, goal->dock_pose.pose.position.y); dock = generateGoalDock(goal); } - // Check if the robot is docked or charging before proceeding - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { - RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock"); + // Check if robot is docked or charging before proceeding, only applicable to charging docks + if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) { + RCLCPP_INFO( + get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock"); return; } @@ -280,9 +281,14 @@ void DockingServer::dockRobot() // Approach the dock using control law if (approachDock(dock, dock_pose)) { // We are docked, wait for charging to begin - RCLCPP_INFO(get_logger(), "Made contact with dock, waiting for charge to start"); + RCLCPP_INFO( + get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); if (waitForCharge(dock)) { - RCLCPP_INFO(get_logger(), "Robot is charging!"); + if (dock->plugin->isCharger()) { + RCLCPP_INFO(get_logger(), "Robot is charging!"); + } else { + RCLCPP_INFO(get_logger(), "Docking was successful!"); + } result->success = true; result->num_retries = num_retries_; stashDockData(goal->use_dock_id, dock, true); @@ -300,7 +306,7 @@ void DockingServer::dockRobot() } catch (opennav_docking_core::DockingException & e) { if (++num_retries_ > max_retries_) { RCLCPP_ERROR(get_logger(), "Failed to dock, all retries have been used"); - throw; + throw e; } RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what()); } @@ -403,7 +409,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & publishDockingFeedback(DockRobot::Feedback::CONTROLLING); // Stop and report success if connected to dock - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { + if (dock->plugin->isDocked() || (dock->plugin->isCharger() && dock->plugin->isCharging())) { return true; } @@ -450,7 +456,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & if (this->now() - start > timeout) { throw opennav_docking_core::FailedToControl( - "Timed out approaching dock; dock nor charging detected"); + "Timed out approaching dock; dock nor charging (if applicable) detected"); } loop_rate.sleep(); @@ -460,6 +466,11 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & bool DockingServer::waitForCharge(Dock * dock) { + // This is a non-charger docking request + if (!dock->plugin->isCharger()) { + return true; + } + rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(wait_charge_timeout_); @@ -589,11 +600,11 @@ void DockingServer::undockRobot() } RCLCPP_INFO( get_logger(), - "Attempting to undock robot from charger of type %s.", dock->getName().c_str()); + "Attempting to undock robot of dock type %s.", dock->getName().c_str()); // Check if the robot is docked before proceeding - if (!dock->isDocked()) { - RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock"); + if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) { + RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock"); return; } @@ -623,7 +634,7 @@ void DockingServer::undockRobot() } // Don't control the robot until charging is disabled - if (!dock->disableCharging()) { + if (dock->isCharger() && !dock->disableCharging()) { loop_rate.sleep(); continue; } @@ -638,7 +649,7 @@ void DockingServer::undockRobot() RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); // Have reached staging_pose vel_publisher_->publish(std::move(command)); - if (dock->hasStoppedCharging()) { + if (!dock->isCharger() || dock->hasStoppedCharging()) { RCLCPP_INFO(get_logger(), "Robot has undocked!"); result->success = true; curr_dock_type_.clear(); @@ -647,7 +658,7 @@ void DockingServer::undockRobot() return; } // Haven't stopped charging? - throw opennav_docking_core::FailedToControl("Failed to control off dock, still charging"); + throw opennav_docking_core::FailedToControl("Failed to control off dock"); } // Publish command and sleep diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 3ff04db47f..298a3be950 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -94,6 +94,7 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); @@ -250,7 +251,7 @@ bool SimpleChargingDock::isDocked() // Find base pose in target frame geometry_msgs::msg::PoseStamped base_pose; base_pose.header.stamp = rclcpp::Time(0); - base_pose.header.frame_id = "base_link"; + base_pose.header.frame_id = base_frame_id_; base_pose.pose.orientation.w = 1.0; try { tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp new file mode 100644 index 0000000000..25e67c5eb9 --- /dev/null +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_util/node_utils.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" + +namespace opennav_docking +{ + +void SimpleNonChargingDock::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) +{ + name_ = name; + tf2_buffer_ = tf; + node_ = parent.lock(); + if (!node_) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Parameters for optional external detection of dock pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_timeout", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_x", rclcpp::ParameterValue(-0.20)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_y", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_yaw", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_pitch", rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(-1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + // Optionally determine if docked via stall detection using joint_states + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_velocity_threshold", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_effort_threshold", rclcpp::ParameterValue(1.0)); + + // If not using stall detection, this is how close robot should get to pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_threshold", rclcpp::ParameterValue(0.05)); + + // Staging pose configuration + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + + node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); + node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); + node_->get_parameter( + name + ".external_detection_translation_x", external_detection_translation_x_); + node_->get_parameter( + name + ".external_detection_translation_y", external_detection_translation_y_); + double yaw, pitch, roll; + node_->get_parameter(name + ".external_detection_rotation_yaw", yaw); + node_->get_parameter(name + ".external_detection_rotation_pitch", pitch); + node_->get_parameter(name + ".external_detection_rotation_roll", roll); + external_detection_rotation_.setEuler(pitch, roll, yaw); + node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); + node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); + node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID + node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); + node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + + // Setup filter + double filter_coef; + 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", 1, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + }); + } + + bool use_stall_detection; + node_->get_parameter(name + ".use_stall_detection", use_stall_detection); + if (use_stall_detection) { + is_stalled_ = false; + node_->get_parameter(name + ".stall_joint_names", stall_joint_names_); + if (stall_joint_names_.size() < 1) { + RCLCPP_ERROR(node_->get_logger(), "stall_joint_names cannot be empty!"); + } + joint_state_sub_ = node_->create_subscription( + "joint_states", 1, + std::bind(&SimpleNonChargingDock::jointStateCallback, this, std::placeholders::_1)); + } + + dock_pose_pub_ = node_->create_publisher("dock_pose", 1); + filtered_dock_pose_pub_ = node_->create_publisher( + "filtered_dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("staging_pose", 1); +} + +geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) +{ + // If not using detection, set the dock pose as the given dock pose estimate + if (!use_external_detection_pose_) { + // This gets called at the start of docking + // Reset our internally tracked dock pose + dock_pose_.header.frame_id = frame; + dock_pose_.pose = pose; + } + + // Compute the staging pose with given offsets + const double yaw = tf2::getYaw(pose.orientation); + geometry_msgs::msg::PoseStamped staging_pose; + staging_pose.header.frame_id = frame; + staging_pose.header.stamp = node_->now(); + staging_pose.pose = pose; + staging_pose.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose.pose.position.y += sin(yaw) * staging_x_offset_; + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, yaw + staging_yaw_offset_); + staging_pose.pose.orientation = tf2::toMsg(orientation); + + // Publish staging pose for debugging purposes + staging_pose_pub_->publish(staging_pose); + return staging_pose; +} + +bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string) +{ + // If using not detection, set the dock pose to the static fixed-frame version + if (!use_external_detection_pose_) { + dock_pose_pub_->publish(pose); + dock_pose_ = pose; + return true; + } + + // If using detections, get current detections, transform to frame, and apply offsets + geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; + + // Validate that external pose is new enough + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + if (node_->now() - detected.header.stamp > timeout) { + RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); + return false; + } + + // Transform detected pose into fixed frame. Note that the argument pose + // is the output of detection, but also acts as the initial estimate + // and contains the frame_id of docking + if (detected.header.frame_id != pose.header.frame_id) { + try { + if (!tf2_buffer_->canTransform( + pose.header.frame_id, detected.header.frame_id, + detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + tf2_buffer_->transform(detected, detected, pose.header.frame_id); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + } + + // Filter the detected pose + detected = filter_->update(detected); + filtered_dock_pose_pub_->publish(detected); + + // Rotate the just the orientation, then remove roll/pitch + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected.pose.orientation; + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); + dock_pose_.pose.orientation = tf2::toMsg(orientation); + + // Construct dock_pose_ by applying translation/rotation + dock_pose_.header = detected.header; + dock_pose_.pose.position = detected.pose.position; + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ - + sin(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ + + cos(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.z = 0.0; + + // Publish & return dock pose for debugging purposes + dock_pose_pub_->publish(dock_pose_); + pose = dock_pose_; + return true; +} + +bool SimpleNonChargingDock::isDocked() +{ + if (joint_state_sub_) { + // Using stall detection + return is_stalled_; + } + + if (dock_pose_.header.frame_id.empty()) { + // Dock pose is not yet valid + return false; + } + + // Find base pose in target frame + geometry_msgs::msg::PoseStamped base_pose; + base_pose.header.stamp = rclcpp::Time(0); + base_pose.header.frame_id = base_frame_id_; + base_pose.pose.orientation.w = 1.0; + try { + tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); + } catch (const tf2::TransformException & ex) { + return false; + } + + // If we are close enough, we are docked + double d = std::hypot( + base_pose.pose.position.x - dock_pose_.pose.position.x, + base_pose.pose.position.y - dock_pose_.pose.position.y); + return d < docking_threshold_; +} + +void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state) +{ + double velocity = 0.0; + double effort = 0.0; + for (size_t i = 0; i < state->name.size(); ++i) { + for (auto & name : stall_joint_names_) { + if (state->name[i] == name) { + // Tracking this joint + velocity += abs(state->velocity[i]); + effort += abs(state->effort[i]); + } + } + } + + // Take average + effort /= stall_joint_names_.size(); + velocity /= stall_joint_names_.size(); + + is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); +} + +} // namespace opennav_docking + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(opennav_docking::SimpleNonChargingDock, opennav_docking_core::ChargingDock) diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index 8ffbe18740..65d6e1d68a 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -60,6 +60,15 @@ target_link_libraries(test_simple_charging_dock simple_charging_dock tf2_geometry_msgs::tf2_geometry_msgs ) +ament_add_gtest(test_simple_non_charging_dock + test_simple_non_charging_dock.cpp +) +ament_target_dependencies(test_simple_non_charging_dock + ${dependencies} +) +target_link_libraries(test_simple_non_charging_dock + ${library_name} simple_non_charging_dock +) # Test Pose Filter (unit) ament_add_gtest(test_pose_filter 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 6bc7439129..7193bb188b 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -48,6 +48,7 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->disableCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); + EXPECT_TRUE(dock->isCharger()); dock->deactivate(); dock->cleanup(); @@ -66,6 +67,8 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); // Below threshold sensor_msgs::msg::BatteryState msg; 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 new file mode 100644 index 0000000000..89c423f5ff --- /dev/null +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +// Testing the simple non-charging dock plugin + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +namespace opennav_docking +{ + +TEST(SimpleNonChargingDockTests, ObjectLifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Check initial states + EXPECT_THROW(dock->isCharging(), std::runtime_error); + EXPECT_THROW(dock->disableCharging(), std::runtime_error); + EXPECT_THROW(dock->hasStoppedCharging(), std::runtime_error); + EXPECT_FALSE(dock->isCharger()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StallDetection) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher( + "joint_states", rclcpp::QoS(1)); + pub->on_activate(); + node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); + std::vector names = {"left_motor", "right_motor"}; + node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); + node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); + + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + + // Stopped, but below effort threshold + sensor_msgs::msg::JointState msg; + msg.name = {"left_motor", "right_motor", "another_motor"}; + msg.velocity = {0.0, 0.0, 0.0}; + msg.effort = {0.0, 0.0, 0.0}; + pub->publish(msg); + rclcpp::Rate r(2); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Moving, with effort + sensor_msgs::msg::JointState msg2; + msg2.name = {"left_motor", "right_motor", "another_motor"}; + msg2.velocity = {1.0, 1.0, 0.0}; + msg2.effort = {5.1, -5.1, 0.0}; + pub->publish(msg2); + rclcpp::Rate r1(2); + r1.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Stopped, with effort + sensor_msgs::msg::JointState msg3; + msg3.name = {"left_motor", "right_motor", "another_motor"}; + msg3.velocity = {0.0, 0.0, 0.0}; + msg3.effort = {5.1, -5.1, 0.0}; + pub->publish(msg3); + rclcpp::Rate r2(2); + r2.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_TRUE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPose) +{ + auto node = std::make_shared("test"); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 0.0, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPoseWithYawOffset) +{ + // Override the parameter default + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.staging_yaw_offset", 3.14}, + } + ); + + auto node = std::make_shared("test", options); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + // Pose should be the same as default, but pointing in opposite direction + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 3.14, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, RefinedPoseTest) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::PoseStamped pose; + + // Timestamps are outdated; this is after timeout + EXPECT_FALSE(dock->isDocked()); + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 0.1; + detected_pose.pose.position.y = -0.5; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + pose.header.frame_id = "my_frame"; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +} // namespace opennav_docking diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index 2f46b8cb76..fb0c93016d 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -31,7 +31,8 @@ DockRobotAction::DockRobotAction( void DockRobotAction::on_tick() { // Get core inputs about what to perform - if (getInput("use_dock_id", goal_.use_dock_id)) { + [[maybe_unused]] auto res = getInput("use_dock_id", goal_.use_dock_id); + if (goal_.use_dock_id) { getInput("dock_id", goal_.dock_id); } else { getInput("dock_pose", goal_.dock_pose); 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 d4566e5836..886fde2fa7 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 @@ -118,6 +118,11 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Gets if this is a charging-typed dock + */ + virtual bool isCharger() {return true;} + std::string getName() {return name_;} protected: 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 new file mode 100644 index 0000000000..9951b19dff --- /dev/null +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -0,0 +1,136 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ + +#include +#include + +#include "opennav_docking_core/charging_dock.hpp" + + +namespace opennav_docking_core +{ + +/** + * @class NonChargingDock + * @brief Abstract interface for a non-charging dock for the docking framework + */ +class NonChargingDock : public ChargingDock +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Virtual destructor + */ + virtual ~NonChargingDock() {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) = 0; + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() = 0; + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() = 0; + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() = 0; + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) = 0; + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id) = 0; + + /** + * @brief Have we made contact with dock? This can be implemented in a variety + * of ways: by establishing communications with the dock, by monitoring the + * the drive motor efforts, etc. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool isDocked() = 0; + + /** + * @brief Are we charging? If a charge dock requires any sort of negotiation + * to begin charging, that should happen inside this function as this function + * will be called repeatedly after the docking loop to check if successful. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool isCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Undocking while current is still flowing can damage a charge dock + * so some charge docks provide the ability to disable charging before the + * robot physically disconnects. The undocking action will not command the + * robot to move until this returns true. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool disableCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Similar to isCharging() but called when undocking. + */ + bool hasStoppedCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Gets if this is a charging-typed dock + */ + bool isCharger() final {return false;} +}; + +} // namespace opennav_docking_core + +#endif // OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_