限速导航

• 概述

• 要求

• 教程步骤

概述

本教程将介绍如何简单使用速度过滤器，该过滤器旨在限制地图上标记的速度限制区域中机器人的最大速度。SpeedFilter 成本地图过滤器插件涵盖了此功能，本文档将启用和使用该插件。

要求

假设 ROS 2、Gazebo 和 TurtleBot3 软件包已在本地安装或构建。请确保 Nav2 项目也在本地构建，因为它是在 构建与安装 中创建的。

教程步骤

1. 准备过滤面罩

正如在 导航概念 中所写，任何 Costmap Filter（包括 Speed Filter）都会读取在过滤器掩码文件中标记的数据。有关过滤器掩码、其类型、详细结构以及如何制作新过滤器掩码的所有信息都写在 使用禁区导航 教程的“1. 准备过滤器掩码”一章中。绘制 Speed Filter 的过滤器掩码的原理与 Keepout Filter 相同（用请求的区域注释地图），只是“OccupancyGrid”掩码值有另一层含义：这些值是地图上单元格对应区域的编码速度限制。

让我们看看它是如何解码的。众所周知， OccupancyGrid 值属于 [0..100] 范围。对于速度过滤器， 0 值表示在掩码上零单元对应的区域中没有速度限制。 [1..100] 范围内的值通过以下公式线性转换为速度限制值：

速度限制 = 滤波掩码数据 * 乘数 + 基数；

where:

• filter_mask_data ——是应限制最大速度的掩模上相应单元的 OccupancyGrid 值。

• base 和 multiplier 是从 Costmap Filter Info Server 发布的 nav2_msgs/CostmapFilterInfo 消息中获取的系数（参见下面的下一章）。

解码后的 speed_limit 值可能具有以下两种含义之一：

• 速度限制以机器人最大速度的百分比表示。

• 速度限制以绝对值表示（例如 m/s）。

速度过滤器使用的含义是从 nav2_msgs/CostmapFilterInfo 消息中读取的。 在本教程中，我们将使用第一种速度限制，以机器人最大速度的百分比表示。

Note

对于以百分比表示的速度限制， speed_limit 将完全用作属于 [0..100] 范围的百分比，而不是 [0.0..1.0] 范围。

创建一个 PGM/PNG/BMP 格式的新图像：将用于世界模拟的 turtlebot3_world.pgm 主地图从 Nav2 存储库复制到新的 speed_mask.pgm 文件。在您最喜欢的光栅图形编辑器中打开 speed_mask.pgm ，并用灰色填充限速区域。在我们的示例中，较深的颜色表示限速较高的区域：

区域 A 填充有 40% 灰色，区域 B 填充有 70% 灰色，这意味着速度限制将在区域 A 中取 100% - 40% = 60% ，在区域 B 中取 100% - 70% = 30% ，该速度限制在该机器人允许的最大速度值范围内。 我们将使用没有阈值的“比例”地图模式。在此模式下，较深的颜色将具有较高的 OccupancyGrid 值。例如，对于灰色为 70% 的区域 B ， OccupancyGrid 数据将等于 70 。因此，为了达到目标，我们需要选择 base = 100.0 和 multiplier = -1.0 。这会将比例 OccupancyGrid 值反转为所需的值。为了方便起见，在 yaml 文件中没有选择任何阈值（ free_thresh 和 occupied_thresh ）：从过滤器蒙版到速度限制百分比的亮度值有 1：1 的全范围转换。

Note

base 和 multiplier 的选择很典型，但不是强制性的。例如，您可以选择地图模式为 raw。在这种情况下，颜色亮度将直接转换为 OccupancyGrid 值。对于以 raw 模式保存的蒙版， base 和 multiplier 将分别等于 0.0 和 1.0。

另一件重要的事情是，没有必要使用整个 “[0..100]” 百分比刻度。可以选择 “base” 和 “multiplier” 系数，以便速度限制值属于百分比范围中间的某个值。例如，“base = 40.0”，“multiplier = 0.1” 将给出 “[40.0%..50.0%]” 范围内的速度限制，步长为 “0.1%”。这可能对微调有用。

所有限速区域都填满后，保存 speed_mask.pgm 图像。

与所有其他地图一样，过滤器蒙版应具有自己的 YAML 元数据文件。将 `turtlebot3_world.yaml <https://github.com/ros-planning/navigation2/blob/main/nav2_bringup/bringup/maps/turtlebot3_world.yaml>”复制到“speed_mask.yaml`_。打开“speed_mask.yaml”并更新字段，如下所示（如前所述，对于“scale”模式，要使用整个颜色亮度范围，不应有阈值：“free_thresh = 0.0”和“occupied_thresh = 1.0”）：

image: turtlebot3_world.pgm
->
image: speed_mask.pgm

mode: trinary
->
mode: scale

occupied_thresh: 0.65
free_thresh: 0.196
->
occupied_thresh: 1.0
free_thresh: 0.0

由于 Costmap2D 不支持方向， origin 向量的最后第三个“偏航”分量应等于零（例如： origin: [1.25, -5.18, 0.0] ）。保存 speed_mask.yaml，新的过滤器掩码即可使用。

Note

世界地图本身和过滤器蒙版可能具有不同的大小、原点和分辨率，这可能会很有用（例如，当过滤器蒙版覆盖地图上较小的区域或一个过滤器蒙版被重复使用多次时，例如为酒店​​中相同形状的房间标注限速区域）。对于这种情况，您还需要更正 YAML 中的“分辨率”和“原点”字段，以便过滤器蒙版正确地放置在原始地图之上。此示例显示使用主地图作为基础，但这不是必需的。

2. 配置 Costmap 过滤器信息发布服务器

每个 costmap 过滤器都会在 nav2_msgs/CostmapFilterInfo 类型的消息中读取传入的元信息（例如过滤器类型或数据转换系数）。这些消息由 Costmap Filter Info Publisher Server 发布。该服务器作为生命周期节点运行。根据 设计文档，nav2_msgs/CostmapFilterInfo 消息与 OccupancyGrid 过滤器掩码主题成对发送。因此，除了 Costmap Filter Info Publisher Server 之外，还应启用一个配置为发布过滤器掩码的 Map Server 新实例。

为了在您的配置中启用 Speed Filter，应在 Python 启动文件中将两个服务器启用为生命周期节点。例如，这可能如下所示，但也可以将它们作为 Composition Node 添加到导航组件容器中：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    # Get the launch directory
    costmap_filters_demo_dir = get_package_share_directory('nav2_costmap_filters_demo')

    # Create our own temporary YAML files that include substitutions
    lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']

    # Parameters
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    mask_yaml_file = LaunchConfiguration('mask')

    # Declare the launch arguments
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true')

    declare_autostart_cmd = DeclareLaunchArgument(
        'autostart', default_value='true',
        description='Automatically startup the nav2 stack')

    declare_params_file_cmd = DeclareLaunchArgument(
            'params_file',
            default_value=os.path.join(costmap_filters_demo_dir, 'params', 'speed_params.yaml'),
            description='Full path to the ROS 2 parameters file to use')

    declare_mask_yaml_file_cmd = DeclareLaunchArgument(
            'mask',
            default_value=os.path.join(costmap_filters_demo_dir, 'maps', 'speed_mask.yaml'),
            description='Full path to filter mask yaml file to load')

    # Make re-written yaml
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file}

    configured_params = RewrittenYaml(
        source_file=params_file,
        root_key=namespace,
        param_rewrites=param_substitutions,
        convert_types=True)

    # Nodes launching commands
    start_lifecycle_manager_cmd = Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_costmap_filters',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': lifecycle_nodes}])

    start_map_server_cmd = Node(
            package='nav2_map_server',
            executable='map_server',
            name='filter_mask_server',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[configured_params])

    start_costmap_filter_info_server_cmd = Node(
            package='nav2_map_server',
            executable='costmap_filter_info_server',
            name='costmap_filter_info_server',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[configured_params])

    ld = LaunchDescription()

    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_mask_yaml_file_cmd)

    ld.add_action(start_lifecycle_manager_cmd)
    ld.add_action(start_map_server_cmd)
    ld.add_action(start_costmap_filter_info_server_cmd)

    return ld

其中， params_file 变量应设置为 YAML 文件，其中包含 Costmap Filter Info Publisher Server 和 Map Server 节点的 ROS 参数。这些参数及其含义列在 Map Server / Saver 页面中。请参阅该页面了解更多信息。 params_file 的示例如下：

costmap_filter_info_server:
  ros__parameters:
    use_sim_time: true
    type: 1
    filter_info_topic: "/costmap_filter_info"
    mask_topic: "/speed_filter_mask"
    base: 100.0
    multiplier: -1.0
filter_mask_server:
  ros__parameters:
    use_sim_time: true
    frame_id: "map"
    topic_name: "/speed_filter_mask"
    yaml_filename: "speed_mask.yaml"

Note, that:

• 对于速度过滤器，以最大速度的百分比设置速度限制，应将 costmap 过滤器的 type 设置为 1。所有可能的 costmap 过滤器类型都可以在 Map Server / Saver 页面中找到。

• 过滤器掩码主题名称应等于 Costmap Filter Info Publisher Server 的 mask_topic 参数和 Map Server 的 topic_name 参数。

• 如上一章所述，为了本教程示例的目的，应将 base 和 multiplier 分别设置为 100.0 和 -1.0。

可以在 navigation2_tutorials 存储库的 nav2_costmap_filters_demo 目录中找到现成的独立 Python 启动脚本、带有 ROS 参数的 YAML 文件和 Speed Filter 的过滤器掩码示例。要简单地运行在 入门指南 中编写的 Turtlebot3 标准模拟上调整的 Filter Info Publisher Server 和 Map Server，请构建演示并启动 costmap_filter_info.launch.py​​，如下所示：

$ mkdir -p ~/tutorials_ws/src
$ cd ~/tutorials_ws/src
$ git clone https://github.com/ros-planning/navigation2_tutorials.git
$ cd ~/tutorials_ws
$ colcon build --symlink-install --packages-select nav2_costmap_filters_demo
$ source ~/tutorials_ws/install/setup.bash
$ ros2 launch nav2_costmap_filters_demo costmap_filter_info.launch.py params_file:=src/navigation2_tutorials/nav2_costmap_filters_demo/params/speed_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/speed_mask.yaml

3. 启用速度过滤器

Costmap Filters 是 Costmap2D 插件。您可以通过在“nav2_params.yaml”中的“plugins”参数中添加“speed_filter”来启用 Costmap2D 中的“SpeedFilter”插件。Speed Filter 插件应定义以下参数：

• plugin: 插件类型。在我们的例子中是 nav2_costmap_2d::SpeedFilter.

• filter_info_topic: 过滤器信息主题名称。这需要等于上一章中 Costmap Filter Info Publisher Server 的 filter_info_topic 参数。

• speed_limit_topic: 发布速度限制的主题名称。

SpeedFilter 支持的完整参数列表列在 Speed Filter Parameters 页面上。

您可以将插件放在 nav2_params.yaml 中的 global_costmap 部分，以将速度限制掩码应用于全局代价地图，或放在 local_costmap 中，以将速度掩码应用于局部代价地图。但是，切勿同时为全局和局部代价地图启用 SpeedFilter 插件。否则，它可能导致在速度限制边界上出现不必要的多个“速度限制”-“无限制”消息链，从而导致机器人抖动或其他不可预测的行为。

在本教程中，我们将为全局代价地图启用速度过滤器。为此，请使用以下配置：

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["speed_filter"]
      ...
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        speed_limit_topic: "/speed_limit"

如 `设计<https://github.com/ros-planning/navigation2/blob/main/doc/design/CostmapFilters_design.pdf>`_ 中所述，速度过滤器发布针对控制器服务器的速度限制 `消息<https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg>`_ ，以便在需要时限制机器人的最大速度。控制器服务器有一个 speed_limit_topic ROS参数，应将其设置为与 speed_filter 插件值相同。地图服务器中的这个主题还可以用于限速区以外的任何其他限速应用，例如根据有效载荷质量动态调整最大速度。

将控制器服务器的“speed_limit_topic”参数设置为与“speed_filter”插件设置的值相同的值：

controller_server:
  ros__parameters:
    ...
    speed_limit_topic: "/speed_limit"

4. 运行 Nav2 堆栈

启动 Costmap Filter Info Publisher Server 和 Map Server 并为全局/本地代价地图启用 Speed Filter 后，按照:ref:getting_started 中的说明运行 Nav2 堆栈：

ros2 launch nav2_bringup tb3_simulation_launch.py

为了更好地可视化速度过滤器掩码，在 RViz 中左侧的“显示”窗格中展开“地图”，并将“主题”从“/map”更改为“/speed_filter_mask”。 将目标设置在限速区域后面，并检查过滤器是否正常工作：机器人在经过限速区域时应减速。下面是它可能的样子（第一张图片显示为全局成本地图启用的速度过滤器，第二张图片显示“speed_mask.pgm”过滤器掩码）：