使用对接服务器

概述

本教程介绍如何将 Docking Server 与 Nav2 机器人系统结合使用。 Docking Server 是一个通用框架，可用于任意类型的机器人和码头，以实现自动对接。 这是通过插件“ChargingDock”和“NonChargingDock”实现的，它们实现了码头的具体功能，例如使用传感器数据检测码头的姿势、如何检测机器人何时与码头接触以及何时成功开始充电。A configuration of the docking server can contain a database of many docks of different plugin ChargingDock and NonChargingDock types to handle a broad range of docking locations and hardware dock revisions. 对接服务器的配置可以包含一个数据库，其中包含许多不同插件“ChargingDock”和“NonChargingDock”类型的码头，以处理广泛的对接位置和硬件码头修订。 包中包含一个示例“SimpleChargingDock”和“SimpleNonChargingDock”插件，其中包含机器人对接非常常见的功能和方法。 它们支持充电站和与静态基础设施（例如传送带）或动态对接（例如托盘）位置的对接。 您很可能也可以使用它，而不必开发自己的 dock 插件来开始。

对接流程如下：

  1. 采取行动请求并获取对接插件及其姿势

  2. 如果机器人不在对接准备姿势的预定容差范围内，则导航至准备姿势

  3. 使用对接插件初步检测对接并返回对接姿势

  4. 进入视觉控制循环，机器人尝试达到对接姿势，同时视觉系统主动对其进行改进

  5. 一旦检测到接触或开始充电，则退出视觉控制循环

  6. 等到充电开始（如果适用）并返回成功。

感谢 NVIDIA 赞助此 Docking Server 软件包和本教程！ 您可以在 nova_carter_docking 软件包 中找到如何使用 Nav2 对接 Nova Carter 机器人以及此操作！

要求

假设 ROS2 和 Nav2 相关包已在本地安装或构建 - 包括“opennav_docking”。 请确保 Nav2 项目也在本地构建，请参阅：ref:“build-instructions” 以获取参考。

请参阅“opennav_docking”自述文件以获取完整的概念解释、参数和 API。

充电底座插件

建立 opennav_docking_core::ChargingDock 和 opennav_docking_core::ChargingDock 插件是为了从通用框架中抽象出机器人和码头的具体内容。 这允许系统利用该框架并提供自己的方法来检测码头的当前姿势、机器人何时充电以及何时接触。 幸运的是，有几种常见的 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() 指示我们是否已在脱离对接时成功停止充电（仅限充电坞站）

SimpleChargingDock 提供了这些 API 的常用选项的实现：

  • getStagingPose - 找到与基座姿势相对偏移的姿势，并进行平移和旋转

  • getRefinedPose - 将检测到的姿势主题（类型为 PoseStamped）过滤到固定框架中 或 如果未启用检测，则为返回基座数据库位置的传递函数

  • isDocked - 如果相对于基座满足姿势公差 或 如果电机的 JointStates 检测到由于驶入基座表面而停转而出现的明显尖峰（如果启用），则返回基座

  • isCharging - 如果 isDocked 或 如果 BatteryState 的电流高于阈值（如果启用）（仅限充电基座），则返回充电

  • disableCharging - 始终为 true，当机器人离开基座时，认为禁用充电是自动的（仅限充电基座）

  • hasStoppedCharging - 的逆``isCharging``（仅限充电座）

因此，对于测试（无检测、无电池信息、无关节状态信息）和实际应用（底座检测、电池状态信息、关节状态信息），可以使用此底座插件。 当只有部分信息可用时，也可以使用它。 如果您的机器人或底座不属于这些实现（即使用无法转换为 ROS 标准类型的自定义电池或检测消息），那么您可能需要构建自己的插件来满足您的特定需求。 但是，您可以使用 SimpleChargingDock，前提是您关闭这些设置并盲目底座以开始使用。 有一个等效的 SimpleNonChargingDock 插件，用于非充电底座需求。

如果您目前没有办法检测您的 dock，可以使用 Apriltags 和 isaac_ros_apriltag 或 ROS image_proc 节点轻松完成 dock 检测。 如果使用 Jetson 平台，请使用 Isaac ROS 获得带有摄像头馈送的 GPU 优化管道。 默认设置开箱即用，请参阅 nova_carter_docking 了解示例。

Note

值得注意的是，您应该提供检测到的基座姿势、充电的电池状态信息以及电机控制器的努力，以实现最高质量和可靠的生产对接。

码头数据库

要停靠您的机器人，您必须在您的环境中提供一组您想要使用的码头。 这是通过停靠服务器中的 码头数据库 完成的，其中包含码头集、其实例类型和一组共享插件。 插件与码头实例分开，因此当空间中可能存在数十个或更多码头时，许多实例可以共享相同的插件以节省内存和网络开销。

必须在对接服务器的配置文件中提供对接插件。 但是，可以在配置文件中或提供的文件路径中提供对接实例，以将服务器的配置与特定的应用程序环境分离。 下面的示例显示了对接插件和对接实例的内联配置，其中一个对接类型（“nova_carter_dock”）指定了 3 个单独的实例：一个主对接和 2 个通用共享后备对接。 只要 TF 知道它们，就可以在任何您喜欢的参考系中将对接指定为“[x, y, theta]”。 请使用您自己的对接插件和地图中的对接位置更新这些。

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 个 dock 插件和 1 个 dock 实例。 Docking Server 的操作 API 可以单独获取 dock 的实例信息以绕过数据库，但其插件必须存在于服务器的配置中。 如果您打算仅使用此 API，则可以设置“dummy_dock”。 一般来说，明智的做法是将您的 dock 设置在数据库中，并使用 Docking Server 的 API 停靠在实例的 Dock ID 上，以将有关 dock 的语义信息与操作请求分离（要求您的应用程序拥有所有 dock 的位置），但绕过数据库对于测试和可移动的停靠目标很有用。

地图中的停靠姿势可以用您最喜欢的地图编辑工具进行注释，通过 rviz2 中的 /clicked_point 获得，或者测量位置。

配置对接服务器

现在，我们既有用于与码头交互的插件，又在地图上指定了码头的位置，就可以配置对接服务器了。 在这个例子中，我们将使用 Nvidia-Segway Nova Carter 机器人，您可以在“nova_carter_docking”包中找到此演示的源代码。 有关参数及其说明的完整列表，请查看：ref:“configuring_docking_server”。

下面是 Nova Carter 机器人使用的示例配置。 值得注意的是，将 fixed_frame 设置为 odom，而不是 map，以便将定位误差与对接过程分离。 我们还对 dock_database 文件中指定的所有 N 个对接使用一个对接插件 nova_carter_dock。

简单的充电底座插件使用与底座数据库姿势相差 70 厘米的暂存偏移来进行暂存。 此暂存姿势选择得足够近以检测底座，但又足够远以提供机动空间来应对预期的底座移动或定位误差。

由于 JointStates 的 use_stall_detection 为 false，因此一旦我们处于对接姿势的 docking_threshold (5cm) 范围内，即视为对接成功。

此对接姿势指定为检测到的姿势，并应用 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],
)

Docking Server 也是一个与 Nav2 中的其他节点一样的可组合节点，因此您也可以使用“LoadComposableNodes/ComposableNode”在 Nav2 进程中启动它。

Docking Action API

对接和脱离对接的 API 相对简单。

DockRobot 操作包含两种主要模式：使用 dock 数据库或指定操作中使用的 dock。 如果使用数据库，请设置 use_dock_id = True``（默认），并且您只需指定要使用的 ``dock_id，例如 home_dock、flex_dock1 或您喜欢的任何 dock 实例。 如果绕过数据库，则必须将 use_dock_id 设置为 false，并且必须完全指定 dock_pose、dock_type 以弥补数据库中缺少的条目元数据。 这要求操作调用者了解所有 dock，而不是将其推送到 Docking Server 的数据库中，这是不推荐的。

或者，如果超出了预暂存公差，您可以使用“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

结果，您可以获得操作是否成功，如果失败，则错误代码是什么，以及尝试重试的总次数。 执行期间，会提供有关当前对接状态的反馈 - 仅在发生事件时不定期发布。它包含状态、尝试对接的当前总用时以及当前重试次数。 如果此信息对您的应用程序有用，则可以从您的操作客户端获取反馈。

UndockRobot 操作甚至更简单。如果在服务器实例未停靠机器人以存储其当前状态信息（例如在停靠后重新启动）时调用取消停靠，则除了 dock_type 之外没有必需的目标字段。 它不包含任何反馈，并且如果发生问题，则返回 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

综合起来

此时，如果您还没有创建 dock 插件（或使用“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”！

祝您停靠愉快！