使用碰撞监视器

../../_images/collision_monitor.gif

概述

本教程介绍如何使用碰撞监视器和 Nav2 堆栈。根据本教程，您可以根据自己的环境和需求进行设置。

要求

假设 ROS2 和 Nav2 依赖包已在本地安装或构建。 请确保 Nav2 项目也在本地构建，因为它是在 构建与安装 中构建的。

配置碰撞监视器

碰撞监测节点有自己的 collision_monitor_node.launch.py​​ 启动文件，并在 collision_monitor_params.yaml 文件中预设参数以供演示，但如果在实践中使用，将其添加到 Nav2 的主启动文件中是微不足道的。 为了演示，将创建两个形状 - 一个内部停止和一个放置在机器人前面的更大的减速边界框：

../../_images/polygons.png

如果减速框内出现 3 个以上的点，机器人将把速度从其值降低到 30%。 当障碍物危险地靠近机器人时，内部停止区域将起作用。 对于此设置，应将以下行添加到 collision_monitor_params.yaml 参数文件中。停止框命名为 PolygonStop，减速边界框命名为 PolygonSlow：

polygons: ["PolygonStop", "PolygonSlow"]
PolygonStop:
  type: "polygon"
  points: "[[0.4, 0.3], [0.4, -0.3], [0.0, -0.3], [0.0, 0.3]]"
  action_type: "stop"
  min_points: 4  # max_points: 3 for Humble
  visualize: True
  polygon_pub_topic: "polygon_stop"
PolygonSlow:
  type: "polygon"
  points: "[[0.6, 0.4], [0.6, -0.4], [0.0, -0.4], [0.0, 0.4]]"
  action_type: "slowdown"
  min_points: 4  # max_points: 3 for Humble
  slowdown_ratio: 0.3
  visualize: True
  polygon_pub_topic: "polygon_slowdown"

Note

可以使用圆形代替多边形，例如对于全向机器人的情况，碰撞可以从任何方向发生。但是，出于教程的需要，我们将重点放在多边形上。出于同样的原因，我们将 Approach 模型排除在范围之外。这两种情况都可以通过参考 碰撞监测 配置指南轻松启用。

本教程中的两种多边形形状都是静态设置的。但是，可以使用包含多边形或轮廓顶点的主题消息随时间动态调整它们。有关更多信息，请参阅配置指南。

对于工作配置，至少应添加一个数据源。 在当前演示中，它使用激光扫描仪（尽管“PointCloud2”和Range/Sonar/IR传感器也是可能的），它由以下几行描述碰撞监视器节点：

observation_sources: ["scan"]
scan:
  type: "scan"
  topic: "scan"

设置主题名称、帧 ID 和超时，以便与默认 Nav2 设置配合使用。 在这种情况下，整个“nav2_collision_monitor/params/collision_monitor_params.yaml”文件将如下所示：

collision_monitor:
  ros__parameters:
    use_sim_time: True
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    transform_tolerance: 0.5
    source_timeout: 5.0
    stop_pub_timeout: 2.0
    enable_stamped_cmd_vel: False
    polygons: ["PolygonStop", "PolygonSlow"]
    PolygonStop:
      type: "polygon"
      points: "[[0.4, 0.3], [0.4, -0.3], [0.0, -0.3], [0.0, 0.3]]"
      action_type: "stop"
      min_points: 4  # max_points: 3 for Humble
      visualize: True
      polygon_pub_topic: "polygon_stop"
    PolygonSlow:
      type: "polygon"
      points: "[[0.6, 0.4], [0.6, -0.4], [0.0, -0.4], [0.0, 0.4]]"
      action_type: "slowdown"
      min_points: 4  # max_points: 3 for Humble
      slowdown_ratio: 0.3
      visualize: True
      polygon_pub_topic: "polygon_slowdown"
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"

使用速度多边形配置碰撞监视器

../../_images/dexory_velocity_polygon.gif

在本教程的这一部分，我们将使用“VelocityPolygon”类型设置碰撞监视器，以执行“停止”操作。“VelocityPolygon”允许用户设置多个多边形以覆盖机器人的速度限制范围。例如，用户可以配置不同的多边形以进行旋转、向前移动或向后移动。碰撞监视器将根据每个子多边形检查机器人的速度，以确定用于碰撞检查的适当多边形。

一般来说，以下是使用“VelocityPolygon”类型配置碰撞监视器的步骤：

  1. 将“VelocityPolygon”添加到“polygons”参数列表

  2. 配置“VelocityPolygon”

  3. 指定多边形的“holonomic”属性（默认为“false”）

  4. 首先在“velocity_polygons”列表中添加一个“stopped”子多边形，以覆盖机器人速度限制的全部范围

  5. 在“velocity_polygons”列表的前面添加额外的子多边形，以覆盖机器人每种运动类型（例如旋转、向前移动、向后移动）的速度限制范围

在这个例子中，我们将考虑一个**非完整**机器人，其线速度限制为 -1.0 到 1.0 m/s，角速度限制为 -1.0 到 1.0 rad/s。子多边形的 linear_min 和 linear_max 参数应设置为机器人的线速度限制，而 theta_min 和 theta_max 参数应设置为机器人的角速度限制。

以下是使用 4 个子多边形覆盖机器人速度限制的全部范围的示例配置：

polygons: ["VelocityPolygonStop"]
VelocityPolygonStop:
  type: "velocity_polygon"
  action_type: "stop"
  min_points: 6
  visualize: True
  enabled: True
  polygon_pub_topic: "velocity_polygon_stop"
  velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
  holonomic: false
  rotation:
    points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
    linear_min: 0.0
    linear_max: 0.05
    theta_min: -1.0
    theta_max: 1.0
  translation_forward:
    points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
    linear_min: 0.0
    linear_max: 1.0
    theta_min: -1.0
    theta_max: 1.0
  translation_backward:
    points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
    linear_min: -1.0
    linear_max: 0.0
    theta_min: -1.0
    theta_max: 1.0
  # This is the last polygon to be checked, it should cover the entire range of robot's velocities
  # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
  # is not covered by any of the other sub-polygons
  stopped:
    points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
    linear_min: -1.0
    linear_max: 1.0
    theta_min: -1.0
    theta_max: 1.0

建议将“已停止”子多边形作为“velocity_polygons”列表中的最后一个条目，以覆盖机器人速度限制的整个范围。如果速度不在任何子多边形的范围内，碰撞监视器将记录一条警告消息并继续使用之前匹配的多边形。

当速度被多个子多边形覆盖时，将使用列表中的第一个子多边形。

For holomic robots:

对于全息机器人，holonomic 属性应设置为 true。在这种情况下，linear_min 和 linear_max 参数应涵盖机器人的合成速度限制，而 theta_min 和 theta_max 参数应涵盖机器人的角速度限制。此外，还有 2 个参数 direction_start_angle 和 direction_end_angle 用于指定合成速度方向。覆盖的方向将始终以**逆时针**方向从 direction_start_angle 跨越到 direction_end_angle。

../../_images/holonomic_direction.png

下面显示了完整机器人的一些常见配置，这些配置涵盖了合成速度的多个方向：

../../_images/holonomic_examples1.png

Preparing Nav2 stack

碰撞监视器设计为在 Nav2 下作为独立安全节点运行。 它充当来自控制器的 cmd_vel 消息的过滤器，以避免潜在的碰撞。 如果没有触发这样的区域，则使用 cmd_vel 消息。 否则，它会根据需要缩放或设置为停止。

默认情况下，碰撞监视器配置为与 Nav2 启动包一起使用，与 navigation_launch.py​​ 启动文件并行运行。为了使碰撞监视器与速度平滑器正确运行，需要在 navigation_launch.py​​ 启动脚本中删除速度平滑器的 cmd_vel_smoothed 重新映射，如下所示。这将使速度平滑器的输出主题不受影响，这将是新添加的碰撞监视器的输入：

Node(
    package='nav2_velocity_smoother',
    executable='velocity_smoother',
    name='velocity_smoother',
    output='screen',
    respawn=use_respawn,
    respawn_delay=2.0,
    parameters=[configured_params],
    arguments=['--ros-args', '--log-level', log_level],
    remappings=remappings +
-           [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+           [('cmd_vel', 'cmd_vel_nav')]),
...
ComposableNode(
    package='nav2_velocity_smoother',
    plugin='nav2_velocity_smoother::VelocitySmoother',
    name='velocity_smoother',
    parameters=[configured_params],
    remappings=remappings +
-              [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+              [('cmd_vel', 'cmd_vel_nav')]),

如果您更改了 Collision Monitor 的默认“cmd_vel_in_topic”和“cmd_vel_out_topic”配置，请确保 Velocity Smoother 的默认输出主题“cmd_vel_smoothed”应与 Collision Monitor 节点的输入速度“cmd_vel_in_topic”参数值匹配，并且输出速度“cmd_vel_out_topic”参数值应为实际的“cmd_vel”以适合替换。

Note

由于碰撞监视器充当安全节点，因此它必须是速度消息后处理链中的最后一个环节，使其成为发布到 cmd_vel 主题的节点。它可以放置在平滑速度之后，如我们的演示中所示，也可以放置在来自控制器服务器的非平滑速度之后，例如，如果系统中未启用速度平滑器，或者在自定义配置中任何其他产生最终速度的模块之后。因此，在任何自定义 Nav2 启动配置中，发布到 cmd_vel 主题的最后一个节点应重新映射以发布到由 cmd_vel_in_topic ROS 参数配置的碰撞监视器输入主题（默认情况下为 cmd_vel_smoothed）。

演示执行

一旦调整了碰撞监控节点并调整了“cmd_vel”主题，碰撞监控节点就可以运行了。 为此，请按照 :ref:“getting_started” 中所述运行 Nav2 堆栈：

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

在并行控制台中，使用启动文件启动 Collision Monitor 节点：

ros2 launch nav2_collision_monitor collision_monitor_node.launch.py

由于“PolygonStop”和“PolygonSlow”多边形都有自己的发布者，因此可以将它们添加到可视化中，如下图所示：

../../_images/polygons_visualization.png

设置初始姿势，然后将 Nav2 目标放在地图上。 机器人将开始移动，在障碍物附近运行时减速，并在靠近障碍物时停止：

../../_images/collision.png