Docking backwards as plugin param

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

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<string> |  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`.
 

nav2_docking/opennav_docking/CMakeLists.txt

@@ -33,8 +33,8 @@ add_library(controller SHARED
 )
 target_include_directories(controller
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(controller PUBLIC
   ${geometry_msgs_TARGETS}
@@ -51,8 +51,8 @@ add_library(${library_name} SHARED
 )
 target_include_directories(${library_name}
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(${library_name} PUBLIC
   angles::angles
@@ -79,8 +79,8 @@ add_library(pose_filter SHARED
 )
 target_include_directories(pose_filter
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(pose_filter PUBLIC
   ${geometry_msgs_TARGETS}
@@ -95,8 +95,8 @@ add_executable(${executable_name}
 )
 target_include_directories(${executable_name}
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 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
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 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
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 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

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 <vector>
+#include <functional>
 #include <memory>
-#include <string>
 #include <mutex>
-#include <functional>
+#include <optional>
+#include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
@@ -50,7 +51,7 @@ class DockingServer : public nav2_util::LifecycleNode
    * @brief A constructor for opennav_docking::DockingServer
    * @param options Additional options to control creation of the node.
    */
-  explicit DockingServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  explicit DockingServer(rclcpp::NodeOptions options = rclcpp::NodeOptions());
 
   /**
    * @brief A destructor for opennav_docking::DockingServer
@@ -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<bool> dock_backwards_;
   // The tolerance to the dock's staging pose not requiring navigation
   double dock_prestaging_tolerance_;
 

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_

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "angles/angles.h"
+#include "nav2_util/node_utils.hpp"
 #include "opennav_docking/docking_server.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/utils.hpp"
@@ -24,22 +25,32 @@
 namespace opennav_docking
 {
 
-DockingServer::DockingServer(const rclcpp::NodeOptions & options)
-: nav2_util::LifecycleNode("docking_server", "", options)
+DockingServer::DockingServer(rclcpp::NodeOptions options)
+: nav2_util::LifecycleNode("docking_server", "",
+    options.automatically_declare_parameters_from_overrides(true))
 {
   RCLCPP_INFO(get_logger(), "Creating %s", get_name());
 
-  declare_parameter("controller_frequency", 50.0);
-  declare_parameter("initial_perception_timeout", 5.0);
-  declare_parameter("wait_charge_timeout", 5.0);
-  declare_parameter("dock_approach_timeout", 30.0);
-  declare_parameter("undock_linear_tolerance", 0.05);
-  declare_parameter("undock_angular_tolerance", 0.05);
-  declare_parameter("max_retries", 3);
-  declare_parameter("base_frame", "base_link");
-  declare_parameter("fixed_frame", "odom");
-  declare_parameter("dock_backwards", false);
-  declare_parameter("dock_prestaging_tolerance", 0.5);
+  nav2_util::declare_parameter_if_not_declared(
+    this, "controller_frequency", rclcpp::ParameterValue(50.0));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "initial_perception_timeout", rclcpp::ParameterValue(5.0));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "wait_charge_timeout", rclcpp::ParameterValue(5.0));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "dock_approach_timeout", rclcpp::ParameterValue(30.0));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "undock_linear_tolerance", rclcpp::ParameterValue(0.05));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "undock_angular_tolerance", rclcpp::ParameterValue(0.05));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "max_retries", rclcpp::ParameterValue(3));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "base_frame", rclcpp::ParameterValue("base_link"));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "fixed_frame", rclcpp::ParameterValue("odom"));
+  nav2_util::declare_parameter_if_not_declared(
+    this, "dock_prestaging_tolerance", rclcpp::ParameterValue(0.5));
 }
 
 nav2_util::CallbackReturn
@@ -57,10 +68,16 @@
   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_);
 
+  if (node->has_parameter("dock_backwards")) {
+    dock_backwards_ = std::make_optional<bool>();
+    get_parameter("dock_backwards", dock_backwards_.value());
+    RCLCPP_WARN(get_logger(), "Parameter dock_backwards is deprecated. "
+      "Please use the dock_direction parameter in your dock plugin instead.");
+  }
+
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
   tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
 
@@ -155,6 +172,7 @@
   curr_dock_type_.clear();
   controller_.reset();
   vel_publisher_.reset();
+  dock_backwards_.reset();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -274,12 +292,17 @@
     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 +335,7 @@
       }
 
       // 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 +432,13 @@
   }
 }
 
-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 +465,7 @@
 
     // 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 +483,7 @@
     // Compute and publish controls
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     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 +530,8 @@
   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 +551,7 @@
     command->header.stamp = now();
     if (getCommandToPose(
         command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
-        !dock_backwards_))
+        !backward))
     {
       return true;
     }
@@ -653,9 +677,13 @@
       // Get command to approach staging pose
       auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
       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