diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 6771397d364..0914e5cbe03 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -439,7 +439,6 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 service_introspection_mode: "disabled" @@ -452,6 +451,7 @@ docking_server: use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18 diff --git a/nav2_docking/README.md b/nav2_docking/README.md index 4e4db71d13f..b063e72dd75 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -207,7 +207,6 @@ For debugging purposes, there are several publishers which can be used with RVIZ | max_retries | Maximum number of retries to attempt | int | 3 | | base_frame | Robot's base frame for control law | string | "base_link" | | fixed_frame | Fixed frame to use, recommended to be a smooth odometry frame **not** map | string | "odom" | -| dock_backwards | Whether the robot is docking with the dock forward or backward in motion | bool | false | | dock_prestaging_tolerance | L2 distance in X,Y,Theta from the staging pose to bypass navigation | double | 0.5 | | dock_plugins | A set of dock plugins to load | vector | N/A | | dock_database | The filepath to the dock database to use for this environment | string | N/A | @@ -251,6 +250,7 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required. | docking_threshold | If not using stall detection, the pose threshold to the docking pose where `isDocked() = true` | double | 0.05 | | staging_x_offset | Staging pose offset forward (negative) of dock pose (m) | double | -0.7 | | 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" | 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 add25f47544..b50b449cbc1 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -33,8 +33,8 @@ add_library(controller SHARED ) target_include_directories(controller PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(controller PUBLIC ${geometry_msgs_TARGETS} @@ -51,8 +51,8 @@ add_library(${library_name} SHARED ) target_include_directories(${library_name} PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(${library_name} PUBLIC angles::angles @@ -79,8 +79,8 @@ add_library(pose_filter SHARED ) target_include_directories(pose_filter PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(pose_filter PUBLIC ${geometry_msgs_TARGETS} @@ -95,8 +95,8 @@ add_executable(${executable_name} ) target_include_directories(${executable_name} PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) @@ -107,8 +107,8 @@ add_library(simple_charging_dock SHARED ) target_include_directories(simple_charging_dock PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(simple_charging_dock PUBLIC ${geometry_msgs_TARGETS} @@ -121,6 +121,7 @@ target_link_libraries(simple_charging_dock PUBLIC tf2_ros::tf2_ros ) target_link_libraries(simple_charging_dock PRIVATE + angles::angles nav2_util::nav2_util_core pluginlib::pluginlib tf2::tf2 @@ -131,8 +132,8 @@ add_library(simple_non_charging_dock SHARED ) target_include_directories(simple_non_charging_dock PUBLIC - "$" - "$" + "$" + "$" ) target_link_libraries(simple_non_charging_dock PUBLIC ${geometry_msgs_TARGETS} @@ -145,6 +146,7 @@ target_link_libraries(simple_non_charging_dock PUBLIC tf2_ros::tf2_ros ) target_link_libraries(simple_non_charging_dock PRIVATE + angles::angles nav2_util::nav2_util_core pluginlib::pluginlib tf2::tf2 diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index cd2ad0c5803..74d46f48f17 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -15,11 +15,12 @@ #ifndef OPENNAV_DOCKING__DOCKING_SERVER_HPP_ #define OPENNAV_DOCKING__DOCKING_SERVER_HPP_ -#include +#include #include -#include #include -#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -87,10 +88,11 @@ class DockingServer : public nav2_util::LifecycleNode * @brief Use control law and dock perception to approach the charge dock. * @param dock Dock instance, gets queried for refined pose and docked state. * @param dock_pose Initial dock pose, will be refined by perception. + * @param backward If true, the robot will drive backwards. * @returns True if dock successfully approached, False if cancelled. For * any internal error, will throw. */ - bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose); + bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward); /** * @brief Wait for charging to begin. @@ -102,9 +104,10 @@ class DockingServer : public nav2_util::LifecycleNode /** * @brief Reset the robot for another approach by controlling back to staging pose. * @param staging_pose The target pose that will reset for another approach. + * @param backward If true, the robot will drive backwards. * @returns True if reset is successful. */ - bool resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose); + bool resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose, bool backward); /** * @brief Run a single iteration of the control loop to approach a pose. @@ -242,7 +245,7 @@ class DockingServer : public nav2_util::LifecycleNode // This is our fixed frame for controlling - typically "odom" std::string fixed_frame_; // Does the robot drive backwards onto the dock? Default is forwards - bool dock_backwards_; + std::optional dock_backwards_; // The tolerance to the dock's staging pose not requiring navigation double dock_prestaging_tolerance_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp index 1acd623f3ee..d0b3dcdf379 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp @@ -24,6 +24,7 @@ #include "nav2_util/geometry_utils.hpp" #include "angles/angles.h" #include "opennav_docking/types.hpp" +#include "opennav_docking_core/charging_dock.hpp" #include "tf2/utils.hpp" namespace utils @@ -33,6 +34,7 @@ using rclcpp::ParameterType::PARAMETER_STRING; using rclcpp::ParameterType::PARAMETER_STRING_ARRAY; using rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; using nav2_util::geometry_utils::orientationAroundZAxis; +using opennav_docking_core::DockDirection; /** * @brief Parse a yaml file to obtain docks @@ -183,6 +185,21 @@ inline double l2Norm(const geometry_msgs::msg::Pose & a, const geometry_msgs::ms delta_angle * delta_angle); } +inline DockDirection getDockDirectionFromString(const std::string & direction) +{ + auto upper_direction = direction; + std::transform( + upper_direction.begin(), upper_direction.end(), upper_direction.begin(), ::toupper); + + if (upper_direction == "FORWARD") { + return DockDirection::FORWARD; + } else if (upper_direction == "BACKWARD") { + return DockDirection::BACKWARD; + } else { + return DockDirection::UNKNOWN; + } +} + } // namespace utils #endif // OPENNAV_DOCKING__UTILS_HPP_ diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 3e85181fe87..09606d1326a 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -38,7 +38,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("max_retries", 3); declare_parameter("base_frame", "base_link"); declare_parameter("fixed_frame", "odom"); - declare_parameter("dock_backwards", false); + declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL); declare_parameter("dock_prestaging_tolerance", 0.5); } @@ -57,10 +57,19 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("max_retries", max_retries_); get_parameter("base_frame", base_frame_); get_parameter("fixed_frame", fixed_frame_); - get_parameter("dock_backwards", dock_backwards_); get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); + bool dock_backwards; + try { + if (get_parameter("dock_backwards", dock_backwards)) { + dock_backwards_ = dock_backwards; + RCLCPP_WARN(get_logger(), "Parameter dock_backwards is deprecated. " + "Please use the dock_direction parameter in your dock plugin instead."); + } + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + } + vel_publisher_ = std::make_unique(node, "cmd_vel", 1); tf2_buffer_ = std::make_shared(node->get_clock()); @@ -155,6 +164,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) curr_dock_type_.clear(); controller_.reset(); vel_publisher_.reset(); + dock_backwards_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -274,12 +284,17 @@ void DockingServer::dockRobot() doInitialPerception(dock, dock_pose); RCLCPP_INFO(get_logger(), "Successful initial dock detection"); + // Get the direction of the movement + bool dock_backward = dock_backwards_.has_value() ? + dock_backwards_.value() : + (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD); + // Docking control loop: while not docked, run controller rclcpp::Time dock_contact_time; while (rclcpp::ok()) { try { // Approach the dock using control law - if (approachDock(dock, dock_pose)) { + if (approachDock(dock, dock_pose, dock_backward)) { // We are docked, wait for charging to begin RCLCPP_INFO( get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); @@ -312,7 +327,7 @@ void DockingServer::dockRobot() } // Reset to staging pose to try again - if (!resetApproach(dock->getStagingPose())) { + if (!resetApproach(dock->getStagingPose(), dock_backward)) { // Cancelled, preempted, or shutting down stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); @@ -409,11 +424,13 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta } } -bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose) +bool DockingServer::approachDock( + Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward) { rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(dock_approach_timeout_); + while (rclcpp::ok()) { publishDockingFeedback(DockRobot::Feedback::CONTROLLING); @@ -440,7 +457,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Make sure that the target pose is pointing at the robot when moving backwards // This is to ensure that the robot doesn't try to dock from the wrong side - if (dock_backwards_) { + if (backward) { target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( tf2::getYaw(target_pose.pose.orientation) + M_PI); } @@ -458,9 +475,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now(); - if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, - dock_backwards_)) - { + if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, backward)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } vel_publisher_->publish(std::move(command)); @@ -507,7 +522,8 @@ bool DockingServer::waitForCharge(Dock * dock) return false; } -bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose) +bool DockingServer::resetApproach( + const geometry_msgs::msg::PoseStamped & staging_pose, bool backward) { rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); @@ -527,7 +543,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin command->header.stamp = now(); if (getCommandToPose( command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, - !dock_backwards_)) + !backward)) { return true; } @@ -653,9 +669,13 @@ void DockingServer::undockRobot() // Get command to approach staging pose auto command = std::make_unique(); command->header.stamp = now(); + bool dock_backward = dock_backwards_.has_value() ? + dock_backwards_.value() : + (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD); + if (getCommandToPose( command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, - !dock_backwards_)) + !dock_backward)) { RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); // Have reached staging_pose diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 298a3be9504..10576a2afff 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -16,6 +16,7 @@ #include "nav2_util/node_utils.hpp" #include "opennav_docking/simple_charging_dock.hpp" +#include "opennav_docking/utils.hpp" namespace opennav_docking { @@ -78,6 +79,10 @@ void SimpleChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + // Direction of docking + nav2_util::declare_parameter_if_not_declared( + node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + node_->get_parameter(name + ".use_battery_status", use_battery_status_); node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); @@ -98,6 +103,13 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + std::string dock_direction; + node_->get_parameter(name + ".dock_direction", dock_direction); + dock_direction_ = utils::getDockDirectionFromString(dock_direction); + if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) { + throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); 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 25e67c5eb94..54001bfe1b6 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -16,6 +16,7 @@ #include "nav2_util/node_utils.hpp" #include "opennav_docking/simple_non_charging_dock.hpp" +#include "opennav_docking/utils.hpp" namespace opennav_docking { @@ -69,6 +70,10 @@ void SimpleNonChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + // Direction of docking + nav2_util::declare_parameter_if_not_declared( + node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + 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( @@ -87,6 +92,13 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + std::string dock_direction; + node_->get_parameter(name + ".dock_direction", dock_direction); + dock_direction_ = utils::getDockDirectionFromString(dock_direction); + if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) { + throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index e96f38bb37f..ff6bbfd5f56 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -62,7 +62,8 @@ def generate_test_description() -> LaunchDescription: 'dock_plugins': ['test_dock_plugin'], 'test_dock_plugin': { 'plugin': 'opennav_docking::SimpleChargingDock', - 'use_battery_status': True}, + 'use_battery_status': True, + 'dock_direction': 'forward'}, 'docks': ['test_dock'], 'test_dock': { 'type': 'test_dock_plugin', @@ -210,7 +211,7 @@ def test_docking_server(self) -> None: # Test docking action self.action_result = [] assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \ - 'dock_robot service not available' + 'dock_robot service not available' goal = DockRobot.Goal() goal.use_dock_id = True 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 dd7d08d39f1..0b7c9ebe7b3 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -39,6 +39,11 @@ class DockingServerShim : public DockingServer { return geometry_msgs::msg::PoseStamped(); } + + std::optional getDockBackward() + { + return dock_backwards_; + } }; TEST(DockingServerTests, ObjectLifecycle) @@ -263,6 +268,50 @@ TEST(DockingServerTests, testDynamicParams) node.reset(); } +TEST(DockingServerTests, testDockBackward) +{ + auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + + // The dock_backwards parameter should be declared but not set + node->on_configure(rclcpp_lifecycle::State()); + EXPECT_FALSE(node->getDockBackward().has_value()); + node->on_cleanup(rclcpp_lifecycle::State()); + + // Now, set the dock_backwards parameter to true + node->set_parameter(rclcpp::Parameter("dock_backwards", rclcpp::ParameterValue(true))); + node->on_configure(rclcpp_lifecycle::State()); + EXPECT_TRUE(node->getDockBackward().has_value()); + EXPECT_TRUE(node->getDockBackward().value()); + node->on_cleanup(rclcpp_lifecycle::State()); + + // Now, set the dock_backwards parameter to false + node->set_parameter(rclcpp::Parameter("dock_backwards", rclcpp::ParameterValue(false))); + node->on_configure(rclcpp_lifecycle::State()); + EXPECT_TRUE(node->getDockBackward().has_value()); + EXPECT_FALSE(node->getDockBackward().value()); + node->on_cleanup(rclcpp_lifecycle::State()); + + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + } // 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 09d8bab0e1a..71873763a1d 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -234,6 +234,34 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock.reset(); } +TEST(SimpleChargingDockTests, GetDockDirection) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward")); + + auto dock = std::make_unique(); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::FORWARD); + dock->cleanup(); + + // Now set to BACKWARD + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward"))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::BACKWARD); + dock->cleanup(); + + // Now set to UNKNOWN + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("other"))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN); + + dock->cleanup(); + dock.reset(); +} + } // 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 5d49b91bea6..96450ef6d05 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 @@ -194,6 +194,34 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock.reset(); } +TEST(SimpleNonChargingDockTests, GetDockDirection) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward")); + + auto dock = std::make_unique(); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::FORWARD); + dock->cleanup(); + + // Now set to BACKWARD + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward"))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::BACKWARD); + dock->cleanup(); + + // Now set to UNKNOWN + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("other"))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN); + + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_utils.cpp b/nav2_docking/opennav_docking/test/test_utils.cpp index 46bd52313f7..986d0b858a3 100644 --- a/nav2_docking/opennav_docking/test/test_utils.cpp +++ b/nav2_docking/opennav_docking/test/test_utils.cpp @@ -135,6 +135,13 @@ TEST(UtilsTests, testl2Norm) EXPECT_NEAR(utils::l2Norm(a, b), 0.734, 1e-3); } +TEST(UtilsTests, testGetDockDirectionFromString) { + using opennav_docking_core::DockDirection; + EXPECT_EQ(utils::getDockDirectionFromString("forward"), DockDirection::FORWARD); + EXPECT_EQ(utils::getDockDirectionFromString("backward"), DockDirection::BACKWARD); + EXPECT_EQ(utils::getDockDirectionFromString("other"), DockDirection::UNKNOWN); +} + } // 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 96ea5ff9370..a225291d804 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -38,6 +38,7 @@ class TestFailureDock : public opennav_docking_core::ChargingDock if (!node_) { throw std::runtime_error{"Failed to lock node"}; } + dock_direction_ = opennav_docking_core::DockDirection::FORWARD; } virtual void cleanup() {} 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 902323ae805..ada25c9f474 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 @@ -26,6 +26,12 @@ namespace opennav_docking_core { +/** + * @enum DockDirection + * @brief An enum class representing the direction of the dock + */ +enum class DockDirection { UNKNOWN, FORWARD, BACKWARD }; + /** * @class ChargingDock * @brief Abstract interface for a charging dock for the docking framework @@ -123,10 +129,18 @@ class ChargingDock */ virtual bool isCharger() {return true;} + /** + * @brief Indicates the direction of the dock. This is used to determine if the + * robot should drive forwards or backwards onto the dock. + * @return DockDirection The direction of the dock + */ + DockDirection getDockDirection() {return dock_direction_;} + std::string getName() {return name_;} protected: std::string name_; + DockDirection dock_direction_{DockDirection::UNKNOWN}; }; } // namespace opennav_docking_core diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 246810868ba..c763808912f 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -106,7 +106,16 @@ controller_server: trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 @@ -309,18 +318,18 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 # Types of docks - dock_plugins: ['simple_charging_dock'] + dock_plugins: ["simple_charging_dock"] simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' + plugin: "opennav_docking::SimpleChargingDock" docking_threshold: 0.05 staging_x_offset: -0.7 use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18 diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index f1941796018..a7593657cee 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -105,7 +105,16 @@ controller_server: trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 @@ -299,18 +308,18 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 # Types of docks - dock_plugins: ['simple_charging_dock'] + dock_plugins: ["simple_charging_dock"] simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' + plugin: "opennav_docking::SimpleChargingDock" docking_threshold: 0.05 staging_x_offset: -0.7 use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18 diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 28c4acba800..aef69d68074 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -105,7 +105,16 @@ controller_server: trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 @@ -299,18 +308,18 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 # Types of docks - dock_plugins: ['simple_charging_dock'] + dock_plugins: ["simple_charging_dock"] simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' + plugin: "opennav_docking::SimpleChargingDock" docking_threshold: 0.05 staging_x_offset: -0.7 use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18 diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index e17800872a3..b81cef7ce56 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -83,10 +83,17 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: [ - "ConstraintCritic", "CostCritic", "GoalCritic", - "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", - "PathAngleCritic", "PreferForwardCritic"] + critics: + [ + "ConstraintCritic", + "CostCritic", + "GoalCritic", + "GoalAngleCritic", + "PathAlignCritic", + "PathFollowCritic", + "PathAngleCritic", + "PreferForwardCritic", + ] ConstraintCritic: enabled: true cost_power: 1 @@ -268,7 +275,8 @@ behavior_server: local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + behavior_plugins: + ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -354,18 +362,18 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 # Types of docks - dock_plugins: ['simple_charging_dock'] + dock_plugins: ["simple_charging_dock"] simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' + plugin: "opennav_docking::SimpleChargingDock" docking_threshold: 0.05 staging_x_offset: -0.7 use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18 @@ -376,9 +384,9 @@ docking_server: filter_coef: 0.1 # Dock instances - docks: ['home_dock'] # Input your docks here + docks: ["home_dock"] # Input your docks here home_dock: - type: 'simple_charging_dock' + type: "simple_charging_dock" frame: map pose: [0.0, 0.0, 0.0] diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index aef72a7825c..311081aaa26 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -354,7 +354,6 @@ docking_server: max_retries: 3 base_frame: "base_link" fixed_frame: "odom" - dock_backwards: false dock_prestaging_tolerance: 0.5 # Types of docks @@ -366,6 +365,7 @@ docking_server: use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false # true + dock_direction: "forward" external_detection_timeout: 1.0 external_detection_translation_x: -0.18