导航速度限制

• 概述

• 必要条件

• 教程指南

概述

本教程展示了如何简单地使用速度过滤器，该过滤器旨在限制地图上标记的速度限制区域中机器人的最大速度。该功能由 SpeedFilter costmap过滤器插件提供，该插件将在本文档中启用。 [待校准@1368]

要求

假设ROS 2、Gazebo和TurtleBot3包是在本地安装或构建的。请确保Nav2项目也是在当地建造的，因为它是在 编译和安装 制造的。 [待校准@1369]

Tutorial Steps

1.准备过滤器掩码

正如 导航相关概念 所写，任何代价地图过滤器 (包括速度过滤器) 都在读取过滤器掩码文件中标记的数据。所有关于过滤面罩、它们的类型、详细结构以及如何制作新面罩的信息都写在 1. Prepare filter masks 章节的 导航禁止区 [校准@mzebra] 教程中。绘制速度过滤器的过滤器掩码的原理与Keepout过滤器的原理相同 (用所请求的区域注释地图)，不同之处在于 OccupancyGrid 掩模值具有另一含义: 这些值是与地图上的单元格相对应的区域的编码速度限制。 [待校准@1370]

让我们，如何解码。我们知道， OccupancyGrid 值属于 [0..100] 范围。速度过滤器 0 价值意味着没有限速区域相应零细胞面具。值 [1..100] 范围线性转换成限速值以下公式: [待校准@1371]

speed_limit = filter_mask_data * multiplier + base;

其中: [校准@小鱼]

• 应限制最大速度的面罩上相应电池的``filter_mask_data`` - is an OccupancyGrid value。 [待校准@1373]

• 参数``base`` and multiplier are coefficients taken from nav2_msgs/CostmapFilterInfo messages由代价地图过滤信息服务器发布 (见下文)。 [待校准@1374]

解码的 speed_limit 值可能有以下两种含义之一: [待校准@1375]

• 速度限制，以机器人最大速度的百分比表示。 [待校准@1376]

• 限速表达绝对值 (例如 m/s )。 [待校准@1377]

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

备注

速度限制表达百分比， speed_limit 将使用完全百分比属于 [0..100] 范围，不 [0.0..1.0] 范围。 [待校准@1379]

创建具有PGM/PNG/BMP格式的新图像: 将将用于世界模拟的 turtlebot3_world.pgm 主图从Nav2存储库复制到新的 speed_mask.pgm 文件中。在你最喜欢的光栅图形编辑器中打开 speed_mask.pgm ，用灰色填充速度限制区域。在我们的例子中，较深的颜色将指示具有更高速度限制的区域: [待校准@1380]

区域 "A" 充满 ”40%” 灰色，区域 "B" -”70%” 灰色，这意味着限速将寸100% 40% = 60% ”在区域 "A" 和” 100% 70% = 30% ”在区域 "B" 最大速度值允许机器人。我们将使用没有阈值的 scale 映射模式。在这种模式下，较暗的颜色将具有较高的 OccupancyGrid 值。例如，对于灰色 OccupancyGrid “70%” 的面积 "B" ，数据将等于 70 。所以为了达到目标，我们需要选择 base = 100.0 和 multiplier = -1.0 。这将把 OccupancyGrid 值的比例反转为期望的比例。为了方便起见，在 yaml 文件中没有选择阈值 ( free_thresh occupied_thresh ): 将亮度值从滤波器掩码-> 到速度限制百分比进行1:1的全范围转换。 [待校准@1381]

备注

这是典型的，但不是 base 和 multiplier 的强制性选择。例如，你可以选择地图模式为 raw 。在这种情况下，颜色亮度直接转化为 OccupancyGrid 值。对于以 raw 模式保存的口罩， base 和 multiplier 相应地等于 0.0 和 1.0 。 [待校准@1382]

另一件重要的事情是没有必要使用整个 [0..100] 百分比量表。可以选择 base 和 multiplier 系数，以便速度限制值属于百分比范围的中间位置。例如， base = 40.0 ， multiplier = 0.1 将从 “[40.0% 37 50.0%] `` range with a step of `` 0.1%” 给出速度限制。这可能对微调有用。 [待校准@1383]

在所有限速区域被填满后，保存 speed_mask.pgm 图像。 [待校准@1384]

像所有其他地图，滤镜蒙版应该自己YAML元数据文件。复制 turtlebot3_world.yaml 转 speed_mask.yaml 。打开 speed_mask.yaml 和更新字段如下所示 (如上所述的 scale 模式整色彩明度范围不应阈值: free_thresh = 0.0 和 occupied_thresh = 1.0 ): [待校准@1385]

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

由于代价地图2D不支持定向， origin 向量的最后第三个 "yaw" 分量应该等于零 (例如: “原点: [1.25，-5.18，0.0]”)。保存 speed_mask.yaml ，新的过滤面罩就可以使用了。 [待校准@1386]

备注

世界地图本身滤镜蒙版可以大小不同，起源和决议可能有用 (e。g.用例时滤镜蒙版覆盖小地区地图或当过滤器掩码用于多次反复，像注释速度禁区相同形状客房)。在这种情况下，您还需要更正YAML中的 resolution 和 origin 字段，以便将过滤器掩码正确放置在原始地图的顶部。此示例说明使用主地图为基础，但不需要。 [待校准@1387]

2.配置成本地图过滤器信息发布服务器 [校准@混沌无形]

每个costmap过滤器读取传入meta信息 (如过滤器类型或数据转换系数) 的 nav2_msgs/CostmapFilterInfo 类型。这些信息由 Costmap Filter Info Publisher Server 出版。服务器作为生命周期节点运行。根据 design document 的说法， nav2_msgs/CostmapFilterInfo 的消息与 OccupancyGrid 的过滤面具主题成对。因此，随着代价地图过滤器信息发布服务器应该启用新实例地图服务器配置为发布过滤器面具。 [待校准@1388]

为了使速度滤波器配置，服务器应启用生命周期节点Python启动文件。例如，这可能如下所示: [待校准@1389]

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文件有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"

请注意: [待校准@1325]

• 速度过滤器设置速度限制百分比最大速度， type 的costmap过滤器应设置 1 。所有可能costmap过滤器类型可以发现在 Map Server / Saver 页面。 [待校准@1390]

• 过滤器掩码的话题名称应与成本地图过滤器信息发布服务器的 mask_topic 参数和地图服务器的``topic_name``参数相同。 [校准@混沌无形]

• 正如描述前一章， base 和 multiplier 应设置 100.0 和 -1.0 因此在本教程示例。 [待校准@1391]

准备去独立Python启动脚本，YAML文件ROS参数过滤器掩码示例速度过滤器可以发现 nav2_costmap_filters_demo 目录 navigation2_tutorials 库。要简单地运行过滤器信息发布服务器和地图服务器，Turtlebot3 在 入门 编写的标准模拟上进行调整，构建演示并按如下方式启动 costmap_filter_info.launch.py : [待校准@1392]

$ 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.启用速度过滤器 [待校准@1393]

代价地图过滤器是Costamp2D插件。您可以在代价地图2D中启用 SpeedFilter 插件，方法是将 speed_filter 添加到 nav2_params.yaml 中的 plugins 参数中。速度过滤器插件应定义以下参数: [待校准@1394]

• plugin: type of plugin. In our case nav2_costmap_2d::SpeedFilter. [待校准@1395]

• 上一章中的代价地图过滤器信息发布者服务器的``filter_info_topic``: filter info topic name. This needs to be equal to filter_info_topic parameter。 [待校准@1396]

• 参数``speed_limit_topic``:名称主题发布速度限制。 [待校准@1397]

全参数列表支持 SpeedFilter 列出在 速度过滤器参数 [待校准@2976] 页面。 [待校准@1398]

你可以插件中 global_costmap 部分 nav2_params.yaml 有速度限制面罩应用于全球costmap或 local_costmap 应用速度面具当地costmap。然而， SpeedFilter 插件不应该同时为全球和本地成本地图启用。否则，它会导致速度限制边界上不必要的多个 "speed restriction" - "no restriction" 信息链，这将导致机器人的抽搐或其他不可预测的行为。 [待校准@1399]

在本教程中，我们将为全局costmap启用速度过滤器。为此，请使用以下配置: [待校准@1400]

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"

正如 design ，过滤风速发布速度制约 messages 针对性的控制器服务器，以便限制最大速度机器人需要。控制器服务器 speed_limit_topic ROS参数，应设置为相同 speed_filter 插件阀门。本课题在地图服务器也可以使用任意数量的速度限制应用超越限制区域，如动态调整最大速度载荷质量。 [待校准@1401]

套装 speed_limit_topic 参数控制器服务器值相同它为 speed_filter 插件: [待校准@1402]

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

4.运行Nav2软件 [校准@混沌无形]

后代价地图过滤器信息发布服务器和地图服务器启动速度滤波器启用全球/本地costmap，运行Nav2堆栈写在 入门 : [待校准@1403]

ros2 launch nav2_bringup tb3_simulation_launch.py

更好的可视化速度滤镜蒙版，RViz在左 Displays 窗格展开 Map 改变 Topic 从 /map -> /speed_filter_mask 。设置目标背后限速和检查过滤器工作正常: 机器人应该减速经历速度制约领域。下面是如何看起来 (图速度过滤器启用全球costmap第二- speed_mask.pgm 滤镜蒙版): [待校准@1404]