使用停靠服务器（Docking Server）

概述
环境要求
ChargingDock 插件
停靠数据库
配置停靠服务器
将停靠服务器添加到启动文件中
停靠动作 API
综合运用

概述

本教程介绍如何在 Nav2 机器人系统中使用停靠服务器（Docking Server）。停靠服务器是一个通用框架，可适配任意类型的机器人和停靠站（dock），实现自动停靠。它通过 ChargingDock 和 NonChargingDock 插件来处理停靠站的具体细节，例如用传感器数据检测停靠站位姿、判断机器人是否与停靠站接触，以及充电是否已成功启动。停靠服务器可配置一个数据库，其中包含多种类型（ChargingDock 和 NonChargingDock）的停靠站，以适应不同的停靠位置和硬件版本。该包附带 SimpleChargingDock 和 SimpleNonChargingDock 两个示例插件，涵盖了机器人停靠中常见的功能和方法，支持充电站、静态基础设施（如传送带）的停靠以及动态停靠位置（如托盘）。大多数场景下可直接使用这些插件，无需自行开发停靠站插件。

停靠流程如下：

接收动作请求，获取停靠站的插件类型及其位姿
若机器人不在停靠站预备位姿（staging pose）的预备容差（prestaging tolerance）范围内，则先导航至预备位姿
通过停靠站插件初步检测停靠站，返回停靠位姿（docking pose）
进入视觉伺服控制循环，视觉系统不断细化停靠位姿，机器人同步尝试到达该位姿
一旦检测到接触或充电开始，即退出控制循环
等待充电开始（如适用）并返回成功

感谢 NVIDIA 赞助本停靠服务器包及教程。如需为 Nova Carter 机器人实现停靠，可参考 nova_carter_docking 包。

环境要求

假定 ROS 2 和 Nav2 的依赖包已安装或已在本地构建，包括 opennav_docking。请确保 Nav2 项目也已本地构建，参考构建说明。

有关完整的概念说明、参数和 API，请参阅 opennav_docking 的 README。

ChargingDock 插件

建立 opennav_docking_core::ChargingDock 和 opennav_docking_core::NonChargingDock 插件的目的是将机器人与停靠站的具体细节从通用框架中抽象出来。这样，系统可以利用该框架，并提供自己的方法来检测停靠站的当前位姿、判断机器人何时开始充电以及何时发生接触。借助几个常用的 ROS API，我们创建了通用性较高的 SimpleChargingDock 和 SimpleNonChargingDock 插件，只要用户提供 JointState、BatteryState 和检测到的停靠站位姿 PoseStamped 话题，即可开箱即用。无论采用哪种方式，系统中每种类型的停靠站都需要配置一个对应的 ChargingDock 或 NonChargingDock 插件。

这些插件有几个关键 API：

PoseStamped getStagingPose(const Pose & pose, const string & frame)，根据停靠站的位置和坐标系提供停靠前的预备位姿
bool getRefinedPose(PoseStamped & pose)，提供检测到的（或直接透传的）停靠站位姿
bool isDocked()，返回机器人是否已与停靠站接触
bool isCharging()，返回停靠后是否已开始充电（仅限充电型停靠站）
bool disableCharging()，若充电由机器人控制，则将其禁用以便离坞（仅限充电型停靠站）
bool hasStoppedCharging()，返回离坞时是否已成功停止充电（仅限充电型停靠站）
bool isCharger()，返回该停靠站是否为充电型
DockDirection getDockDirection()，返回停靠站的方向（机器人应向前、向后等驶入停靠站）
bool shouldRotateToDock()，返回机器人是否应旋转以停靠（例如在无检测的情况下进行倒车停靠）

SimpleChargingDock 为这些 API 提供了涵盖常见场景的实现：

getStagingPose——根据停靠站位姿计算带有平移和旋转偏移的相对位姿
getRefinedPose——将 PoseStamped 类型的检测位姿话题过滤到固定坐标系（fixed frame），或者在未启用检测时直接返回停靠站的数据库位置
isDocked——当与停靠站的位姿偏差在容差范围内时返回已停靠；若启用了失速检测，则在 JointStates 检测到机器人驶入停靠站表面产生明显失速尖峰时返回已停靠
isCharging——当 isDocked 为真，或者（若启用）BatteryState 的电流高于阈值时返回充电中（仅限充电型停靠站）
disableCharging——始终返回 true，即假定机器人离开停靠站时充电会自动停止（仅限充电型停靠站）
hasStoppedCharging——与 isCharging 相反（仅限充电型停靠站）

因此，该停靠站插件既可用于测试（无检测、无电池信息、无关节状态信息），也可用于实际应用（停靠站检测、电池状态信息、关节状态信息），即使只有部分信息可用也能正常工作。如果你的机器人或停靠站不符合上述实现方式（例如使用无法转换为 ROS 标准类型的自定义电池或检测消息），则需自行开发插件以满足特定需求。不过，你也可以关闭相关设置、进行盲停（blind dock）来快速上手 SimpleChargingDock。对于非充电型停靠需求，还有一个等效的 SimpleNonChargingDock 插件。

如果目前没有检测停靠站的方法，可以使用 AprilTags 配合 isaac_ros_apriltag 或 ROS image_proc 节点轻松实现停靠站检测。若使用 Jetson 平台，推荐使用 Isaac ROS，可获得针对摄像头输入的 GPU 优化流水线。默认配置已对此提供支持，示例参见 nova_carter_docking。

注意：要在生产环境中获得最高质量和最可靠的停靠效果，应提供检测到的停靠站位姿、充电所需的电池状态信息以及电机控制器力矩。

停靠数据库

要让机器人停靠，需要提供环境中使用的停靠站集合。这在停靠服务器中通过*停靠数据库（Dock Database）*来完成，其中包含停靠站集合、各自的实例类型以及一组共享插件。插件与停靠站实例分离，使多个实例可以共享同一插件，从而节省内存和网络开销——当一个空间中存在数十个甚至更多停靠站时，这一点尤为重要。

停靠站插件必须在停靠服务器的配置文件中指定。停靠站实例则可以在配置文件中内联指定，或者通过独立文件路径提供，从而将服务器配置与具体应用环境解耦。下面的示例展示了停靠插件和停靠站实例的内联配置，指定了一种停靠站类型（nova_carter_dock），包含 3 个实例：一个主停靠站（home dock）和 2 个通用备用停靠站。停靠站位姿可以用 [x, y, theta] 格式指定在任意参考坐标系中，只要 TF 能解析即可。请根据实际的停靠插件和地图中的停靠位置修改这些内容。

  docking_server:
    ros__parameters:
      # Types of docks
      dock_plugins: ['nova_carter_dock']
      nova_carter_dock:
        plugin: 'opennav_docking::SimpleChargingDock'
        # More parameters exist here that we will discuss later in the tutorial

      # Dock instances
      docks: ['home_dock','flex_dock1', 'flex_dock2']
      home_dock:
        type: 'nova_carter_dock'
        frame: map
        pose: [0.0, 0.0, 0.0]
      flex_dock1:
        type: 'nova_carter_dock'
        frame: map
        pose: [10.0, 10.0, 0.0]
      flex_dock2:
        type: 'nova_carter_dock'
        frame: map
        pose: [30.0, 30.0, 0.0]

      # Or use
      # dock_database: /my/path/to/dock_database.yaml

下面是对应的独立 dock_database.yaml 文件内容，可通过 dock_database 参数提供给 docking_server。

  docks:
    home_dock:
      type: "nova_carter_dock"
      frame: "map"
      pose: [0.0, 0.0, 0.0]
    flex_dock1:
      type: "nova_carter_dock"
      frame: "map"
      pose: [10.0, 10.0, 0.0]
    flex_dock2:
      type: "nova_carter_dock"
      frame: "map"
      pose: [20.0, 20.0, 0.0]

至少需要提供 1 个停靠站插件和 1 个停靠站实例。停靠服务器的动作 API（Action API）可以单独接收停靠站实例信息来绕过数据库，但所用插件的类型必须存在于服务器配置中。如果打算只用这个 API，可以设置一个 dummy_dock。通常建议将停靠站信息存储在数据库中，通过 Dock ID 调用 API 进行停靠，这样可以将停靠站的语义信息与动作请求解耦（否则应用程序需要自行保存所有停靠站位置）。不过，在测试或停靠目标可移动的场景下，绕过数据库也很实用。

地图中的停靠站位姿可用建图编辑工具标注、通过 RViz2 的 /clicked_point 获取，或直接使用测量值。

配置停靠服务器

有了与停靠站交互的插件，并指定了地图中的停靠站位置后，就可以开始配置停靠服务器了。本示例使用 NVIDIA-Segway Nova Carter 机器人，演示源码可在 nova_carter_docking 包中找到。完整的参数列表及说明，请参阅停靠服务器配置。

下面是 Nova Carter 机器人的示例配置。注意 fixed_frame 设置为 odom 而非 map，以将定位误差与停靠过程解耦。 dock_database 文件中指定的所有 N 个停靠站都使用同一个停靠站插件 nova_carter_dock。

简单充电停靠站插件使用距停靠站数据库位姿 70 厘米的预备偏移（staging offset）进行预定位。该预备位姿的选择原则是：足够近以确保能检测到停靠站，又足够远以留出机动空间，应对停靠站移动或定位误差。

由于 use_stall_detection 设置为 false，当机器人到达距停靠位姿 docking_threshold（5 厘米）以内时即判定停靠成功。停靠位姿取自检测结果并应用 external_detection_* 偏移，以修正机器人相对于检测特征的目标停靠位姿。本示例使用 AprilTags，因此对 AprilTag 检测坐标系施加旋转，并施加 -0.18 的平移偏移，以匹配机器人停靠时相对于标签应处的位姿。由于 use_external_detection_pose 和 use_battery_status 均已启用，判断是否充电时会同时参考检测到的停靠站位姿（AprilTag）和电池状态信息。

最大速度设为 15 厘米/秒，缓慢倒车驶入停靠站；最多重试 3 次，以应对停靠过程中未检测到充电或丢失停靠站跟踪的情况。

    docking_server:
      ros__parameters:
        controller_frequency: 50.0
        initial_perception_timeout: 5.0
        wait_charge_timeout: 5.0
        dock_approach_timeout: 30.0
        undock_linear_tolerance: 0.05
        undock_angular_tolerance: 0.1
        max_retries: 3
        base_frame: "base_link"
        fixed_frame: "odom"
        dock_backwards: false
        dock_prestaging_tolerance: 0.5

        # Types of docks
        dock_plugins: ['nova_carter_dock']
        nova_carter_dock:
          plugin: 'opennav_docking::SimpleChargingDock'
          docking_threshold: 0.05
          staging_x_offset: -0.7
          use_external_detection_pose: true
          use_battery_status: true
          use_stall_detection: false

          external_detection_timeout: 1.0
          external_detection_translation_x: -0.18
          external_detection_translation_y: 0.0
          external_detection_rotation_roll: -1.57
          external_detection_rotation_pitch: -1.57
          external_detection_rotation_yaw: 0.0
          filter_coef: 0.1

        # Sep. file of dock instances so config file can be used in multiple locations
        dock_database: /my/path/to/dock_database.yaml

        controller:
          k_phi: 3.0
          k_delta: 2.0
          v_linear_min: 0.15
          v_linear_max: 0.15

将停靠服务器添加到启动文件中

现在可以将该服务器及其参数文件路径添加到启动文件中（或添加到主共享配置文件中）。

    nova_carter_dock_params_dir = os.path.join(
            get_package_share_directory('nova_carter_docking'), 'params')
    params_file = default_value=os.path.join(nova_carter_dock_params_dir, 'nova_carter_docking.yaml')

    docking_server = Node(
        package='opennav_docking',
        executable='opennav_docking',
        name='docking_server',
        output='screen',
        parameters=[params_file],
    )

注意：停靠服务器与 Nav2 中的其他节点一样，也是可组合节点（composable node），因此可以在 Nav2 进程内通过 LoadComposableNodes/ComposableNode 启动。

停靠动作 API

停靠和离坞（undocking）的 API 比较简单。

DockRobot 动作有两种主要模式：使用停靠数据库，或在动作请求中直接指定停靠站。使用数据库时，将 use_dock_id 设为 True（默认值），只需指定要使用的 dock_id，例如 home_dock、flex_dock1 或其他任意停靠站实例。绕过数据库时，需将 use_dock_id 设为 false，并完整指定 dock_pose 和 dock_type，以替代数据库中条目的元数据。这要求动作调用方知晓所有停靠站信息，而非将其交给停靠服务器的数据库管理，因此不推荐这种做法。

还可以通过 navigate_to_staging_pose = False 禁用 Nav2 导航到预备位姿的功能（当超出预备容差时），或通过 max_staging_time 设置预备导航的最长时间。

  #goal definition
  bool use_dock_id True  # Whether to use the dock_id or dock_pose fields
  string dock_id  # Dock name or ID to dock at, from given dock database

  geometry_msgs/PoseStamped dock_pose  # Dock pose
  string dock_type  # If using dock_pose, what type of dock it is. Not necessary if only using one type of dock.

  float32 max_staging_time 1000.0  # Maximum time for navigation to get to the dock's staging pose.
  bool navigate_to_staging_pose True  # Whether or not to navigate to staging pose or assume robot is already at staging pose within tolerance to execute behavior

  ---
  #result definition
  bool success True  # docking success status
  uint16 error_code 0  # Contextual error code, if any
  uint16 num_retries 0  # Number of retries attempted

  ---
  #feedback definition
  uint16 state  # Current docking state
  builtin_interfaces/Duration docking_time  # Docking time elapsed
  uint16 num_retries 0  # Number of retries attempted

结果（result）中包含动作是否成功、失败时的错误码以及重试总次数。执行过程中会发布当前停靠状态的反馈——仅在事件发生时不定期发布，包含当前状态、本次停靠尝试已用时长以及重试次数。如有需要，可从动作客户端获取这些反馈信息。

UndockRobot 动作更简单。除了在服务器不知道机器人当前停靠状态（例如在停靠站上重启后）时调用离坞需要 dock_type 外，没有其他必需的 goal 字段。它不包含反馈，只返回 success 状态，出错时返回 error_code。

  #goal definition
  string dock_type
  float32 max_undocking_time 30.0 # Maximum time to undock

  ---
  #result definition
  bool success True  # docking success status
  uint16 error_code 0  # Contextual error code, if any

  ---
  #feedback definition

综合运用

如果尚未完成，请先创建停靠站插件（或直接使用 SimpleChargingDock）、配置文件和启动文件，以及 AprilTags 或其他检测器等必需节点。 nova_carter_docking 包中提供了本教程的完整示例，包含配置文件和启动文件，后者集成了 AprilTags 检测器和 PoseStamped 位姿发布器。

如果使用 AprilTags 和 NVIDIA Jetson，可在 media/ 目录中找到所用标签，以及完成全部设置的启动文件 isaac_apriltag_detection_pipeline.launch.py。若不使用 Jetson，可用 image_proc 替代 Isaac ROS AprilTag 检测器。

可使用 nova_carter_docking 根目录下的 demo.py 脚本进行测试。该脚本会虚拟地将机器人位姿设为停靠站的预备位姿，跳过导航阶段直接尝试停靠，然后循环执行停靠和离坞。这在首次设置时非常有用，可用于测试停靠、优化检测偏移，并评估整个系统的可靠性。下面是实际运行的视频：

可以看到，机器人能够应对以下情况：

距停靠站预备位姿较远，只要停靠站在视野内即可完成停靠
检测停靠站偏移并计算控制量以成功停靠——包括运行期间或运行之间手动移动停靠站的情况
借助检测和充电状态反馈，实现 100% 成功率的反复停靠

该脚本演示了停靠服务器的基本用法，但并未使用预建图的停靠位置数据库。启动 Nav2 并在地图中完成定位后，可以调整 dockRobot()，使其接收指定的 dock_id 进行停靠，从而在真实环境中运行完整的停靠系统：

    def dockRobot(self, dock_id = ""):
        """Send a `DockRobot` action request."""
        print("Waiting for 'DockRobot' action server")
        while not self.docking_client.wait_for_server(timeout_sec=1.0):
            print('"DockRobot" action server not available, waiting...')

        goal_msg = DockRobot.Goal()
        goal_msg.use_dock_id = True
        goal_msg.dock_id = dock_id  # if wanting to use ID instead

        print('Docking at ID: ' + str(dock_id) + '...')
        send_goal_future = self.docking_client.send_goal_async(goal_msg,
                                                                self._feedbackCallback)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handle = send_goal_future.result()

        if not self.goal_handle.accepted:
            print('Docking request was rejected!')
            return False

        self.result_future = self.goal_handle.get_result_async()
        return True

    ...

    dock_id = 'home_dock'
    tester.dockRobot(dock_id)

根据机器人相对于停靠站的位姿以及预备容差的设置，Nav2 可能会在停靠前先导航到预备位姿。若要禁用此行为，可设置 goal_msg.navigate_to_staging_pose = False，使停靠立即执行。上述视频展示了这两种情况的效果。

如果不想通过 Python 或 C++ 脚本调用停靠服务器，而是想在自主行为树中使用，可查看 opennav_docking_bt 中的 DockRobot 和 UndockRobot 行为树节点——它们可以从应用行为树中调用停靠服务器，并附带了一个 XML 示例。注意，当 navigate_to_staging_pose = True 时，不能从 Nav2 行为树内部调用 DockRobot（因为它会递归调用 Nav2），只能从更高层级的自主行为树中调用。若要从 Nav2 BT 内部调用 DockRobot，需先将机器人大致预定位到停靠站附近（作为导航目标这很容易实现）。不过，UndockRobot 可以随时从任何行为树中调用。

祝停靠顺利！