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"      |
 
 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/simple_charging_dock.hpp

@@ -34,6 +34,8 @@ namespace opennav_docking
 class SimpleChargingDock : public opennav_docking_core::ChargingDock
 {
 public:
+  using DockDirection = opennav_docking_core::DockDirection;
+
   /**
    * @brief Constructor
    */
@@ -103,6 +105,13 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
    */
   virtual bool hasStoppedCharging();
 
+  /**
+   * @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
+   */
+  virtual DockDirection getDockDirection();
+
 protected:
   void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
 
@@ -146,6 +155,8 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
   // Offset for staging pose relative to dock pose
   double staging_x_offset_;
   double staging_yaw_offset_;
+  // Does the robot drive backwards onto the dock? Default is forwards
+  std::string dock_direction_;
 
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;

nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp

@@ -34,6 +34,8 @@ namespace opennav_docking
 class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
 {
 public:
+  using DockDirection = opennav_docking_core::DockDirection;
+
   /**
    * @brief Constructor
    */
@@ -88,6 +90,13 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
    */
   virtual bool isDocked();
 
+  /**
+   * @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
+   */
+  virtual DockDirection getDockDirection();
+
 protected:
   void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
 
@@ -124,6 +133,8 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
   // Offset for staging pose relative to dock pose
   double staging_x_offset_;
   double staging_yaw_offset_;
+  // Does the robot drive backwards onto the dock? Default is forwards
+  std::string dock_direction_;
 
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;

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,17 @@ inline double l2Norm(const geometry_msgs::msg::Pose & a, const geometry_msgs::ms
     delta_angle * delta_angle);
 }
 
+inline DockDirection getDockDirectionFromString(const std::string & direction)
+{
+  if (direction == "forward") {
+    return DockDirection::FORWARD;
+  } else if (direction == "backward") {
+    return DockDirection::BACKWARD;
+  } else {
+    return DockDirection::UNKNOWN;
+  }
+}
+
 }  // namespace utils
 
 #endif  // OPENNAV_DOCKING__UTILS_HPP_