diff --git a/configuration/packages/configuring-docking-server.rst b/configuration/packages/configuring-docking-server.rst index df8ad5addc..4d126f73da 100644 --- a/configuration/packages/configuring-docking-server.rst +++ b/configuration/packages/configuring-docking-server.rst @@ -82,7 +82,18 @@ Parameters ============== ============== Description - Angular Tolerance (rad) to exist undocking loop at staging pose. + Angular tolerance (rad) to exit undocking loop at staging pose. + +:rotation_angular_tolerance: + + ============== ============== + Type Default + -------------- -------------- + double 0.05 + ============== ============== + + Description + Angular tolerance (rad) to exit the rotation loop when rotate_to_dock is enabled. :max_retries: @@ -117,6 +128,17 @@ Parameters Description Fixed frame to use, recommended to be a smooth odometry frame **not** map. +:odom_topic: + + ============== ============== + Type Default + -------------- -------------- + string "odom" + ============== ============== + + Description + The topic to use for the odometry data when rotate_to_dock is enabled. + :dock_backwards: ============== ============== @@ -283,6 +305,28 @@ Parameters Description Radius to end goal to commense slow down. +:controller.rotate_to_heading_angular_vel: + + ============== ============== + Type Default + -------------- -------------- + double 1.0 + ============== ============== + + Description + Angular velocity (rad/s) to rotate to the goal heading when rotate_to_dock is enabled. + +:controller.rotate_to_heading_max_angular_accel: + + ============== ============== + Type Default + -------------- -------------- + double 3.2 + ============== ============== + + Description + Maximum angular acceleration (rad/s^2) to rotate to the goal heading when rotate_to_dock is enabled. + :controller.use_collision_detection: ============== ============== @@ -389,7 +433,7 @@ Simple Charging Dock is a provided charging dock plugin that can handle many doc ============== ============== Description - Staging pose angle relative to dock pose (rad). + Staging pose angle relative to dock pose (rad). If ``dock_direction`` is set to "backward", this angle must be faced in the opposite direction of the dock pose. However, if ``rotate_to_dock`` is enabled, this angle must be facing the same direction as the dock pose because the robot will rotate to the dock pose after detection. :.use_battery_status: @@ -557,7 +601,7 @@ Simple Charging Dock is a provided charging dock plugin that can handle many doc Description If not using stall detection, the pose threshold to the docking pose where ``isDocked() = true``. -::dock_direction: +:.dock_direction: ============== ============== Type Default @@ -568,6 +612,19 @@ Simple Charging Dock is a provided charging dock plugin that can handle many doc Description Whether the robot is docking with the dock forward or backward in motion. This is the replacement for the deprecated ``dock_backwards`` parameter. Options are "forward" or "backward". +:.rotate_to_dock: + + ============== ============== + Type Default + -------------- -------------- + bool false + ============== ============== + + Description + Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. In the undocking phase, the robot will move forward to the staging pose and then rotate to the original heading. This may also be paired with sensor detection in the reverse direction as well if available. + + Note: This parameter is only valid when the ``dock_direction`` is set to "backward". + Example ******* .. code-block:: yaml @@ -583,6 +640,8 @@ Example max_retries: 3 base_frame: "base_link" fixed_frame: "odom" + odom_topic: "odom" + dock_backwards: false # Deprecated, use dock_direction in plugin dock_prestaging_tolerance: 0.5 service_introspection_mode: "disabled" @@ -595,6 +654,7 @@ Example use_external_detection_pose: true use_battery_status: false # true use_stall_detection: false + rotate_to_dock: false external_detection_timeout: 1.0 external_detection_translation_x: -0.18 @@ -618,3 +678,14 @@ Example k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + v_angular_max: 0.75 + slowdown_radius: 0.25 + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_max_angular_accel: 3.2 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 1.0 + simulation_time_step: 0.1 + dock_collision_threshold: 0.3 diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index dc9fdfa3f2..75b984f575 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -477,3 +477,20 @@ New Position Goal Checker ************************* In `PR #5162 `_, a new goal checker plugin called ``PositionGoalChecker`` has been added to Nav2. This plugin provides an alternative to the existing goal checkers by focusing only on the position component of the robot pose, ignoring orientation. This is used by the RPP controller to create stateful behavior to rotate to heading once meeting the goal tolerance. + +Docking backwards without sensor +******************************** + +In `PR #5153 `_, the docking server was updated to allow docking backwards without a sensor for detection. It should be setup to approach the staging pose for the dock in the forward direction with sensor coverage for dock detection, then after detection it will rotate to back up into the dock using the initial detected pose only for deadreckoning. + +A new parameter ``reverse_to_dock`` was added to the ``SimpleChargingDock`` and ``SimpleNonChargingDock`` plugins to allow this feature. + +Default value: + +- false + +See :ref:`configuring_docking_server` for more information. + +Here we can see the working demo of the feature: + +.. image:: images/reverse_to_dock.gif diff --git a/migration/images/reverse_to_dock.gif b/migration/images/reverse_to_dock.gif new file mode 100644 index 0000000000..7196420000 Binary files /dev/null and b/migration/images/reverse_to_dock.gif differ diff --git a/tutorials/docs/using_docking.rst b/tutorials/docs/using_docking.rst index 7705a8ef9e..1c1ac7793b 100644 --- a/tutorials/docs/using_docking.rst +++ b/tutorials/docs/using_docking.rst @@ -67,6 +67,9 @@ The plugins has a few key APIs: - ``bool isCharging()`` which provides if we've started charging while docked (charging docks only) - ``bool disableCharging()`` which should disable charging, if under the robot's control for undocking (charging docks only) - ``bool hasStoppedCharging()`` which indicates if we've successfully stopped charging on undocking (charging docks only) +- ``bool isCharger()`` which indicates if this is a charging-typed dock +- ``DockDirection getDockDirection()`` which indicates the direction of the dock (if the robot should drive forwards, backwards, etc onto the dock) +- ``bool shouldRotateToDock()`` which indicates if the robot should rotate to dock (for example, to perform a backward docking without detections) The ``SimpleChargingDock`` provides an implementation with common options for these APIs: