ROS2与Navigation2入门教程-使用速度限制进行导航

说明：

介绍如何使用速度限制进行导航

概述

本教程会说明如何简单地使用速度过滤器，该过滤器设计用于限制机器人在地图上标记的限速区中的最大速度。
该功能由SpeedFilter成本地图过滤器插件提供，该插件将会在本教程中启用和使用

要求

本教程假设已经在本地机器上安装或者构建好了ROS 2、Gazebo和TurtleBot3软件包。
请确保Nav2项目也是在本地构建的

具体步骤

准备过滤器掩码
正如“移动机器人导航概念”教程中所述，任何成本地图过滤器（包括速度过滤器）都是在读取过滤器掩码文件中标记的数据。
关于过滤器掩码的概念及其类型、详细结构和制作新过滤器掩码的方法等所有信息都已经在“使用禁区进行导航”教程中的“1-准备过滤器掩码”一节中详细介绍过了。
现在来看下是如何被解码的。已经知道OccupancyGrid值介于 [0..100]范围之内。
对于速度过滤器，0值表示对应于掩码上零值单元格的区域中没有速度限制。
[1..100]范围内的值可以通过以下公式线性转换为速度限制值：

speed_limit = filter_mask_data * multiplier + base

filter_mask_data——掩码上应限制最大速度的相应单元格的 OccupancyGrid值。
base和multiplier——从成本地图过滤器信息发布者服务器（参见下一节）上发布的nav2_msgs/CostmapFilterInfo消息中获取的系数。
解码的speed_limit值可能具有以下两种含义之一：
- 以机器人最大速度的百分比表示的速度限制。
- 以绝对值表示的速度限制（例如以m/s为单位）。
速度过滤器具体使用哪种含义是从nav2_msgs/CostmapFilterInfo消息中读取的。
本教程中将会使用第一种速度限制，以机器人最大速度的百分比表示
对于以百分比表示的速度限制，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%。这里将会使用没有阈值的scale地图模式。在这种模式下，较深的颜色会具有较高的OccupancyGrid值。
例如，对于区域“B”，具有70%灰色的OccupancyGrid数据会等于70。因此，为了达到目标，需要选用系数base = 100.0和multiplier = -1.0。这样会将OccupancyGrid比例值反转成所需的值。
为方便起见，在yaml文件中没有选择阈值free_thresh和occupied_thresh，这样就会将来自过滤器掩码的亮度值1:1全范围转换为速度限制百分比值。
这是通常的做法，但并不强制要求选择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%（而不是1%）。这对于微调可能很有用。
填充好所有限速区后，保存speed_mask.pgm图像。
与所有其他地图一样，过滤器掩码应该有自己的YAML元数据文件。将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向量的最后第三个分量“偏航角（yaw）”应该等于零。
例如：origin：[1.25, -5.18, 0.0]。
保存speed_mask.yaml文件，这样新的过滤器掩码准备就绪可以使用了
机器人世界地图本身和过滤器掩码可能具有不同的尺寸、原点和分辨率，这可能会很有用，例如当过滤器掩码覆盖地图上较小区域时的情况，或者当多次重复使用同一个过滤器掩码的情况（例如为酒店中相同形状的房间注释限速区）。
对于这种情况，还需要更正 YAML文件中的resolution和origin字段，以便过滤器掩码能够正确地放置在原始地图的上面。
上面这个示例过滤器掩码展示了使用主地图作为基础地图的情况，但这并不是必需的

配置成本地图过滤器信息发布者服务器

每个成本地图过滤器都会在一条nav2_msgs/CostmapFilterInfo类型的消息中读取传入的元信息（例如过滤器类型或数据转换系数）。这些消息会由成本地图过滤器信息发布者服务器（Costmap Filter Info Publisher Server）发布。
该服务器是作为一个生命周期节点来运行的。根据其设计文档，nav2_msgs/CostmapFilterInfo消息与OccupancyGrid过滤器掩码话题会成对出现。
因此，在启用成本地图过滤器信息发布者服务器的同时，还应该启用一个新的被配置成发布过滤器掩码的地图服务器（Map Server）实例。
为了在配置中启用速度过滤器，这两个服务器都应该在Python启动文件中启用为生命周期节点。
例如，这可能会如下面这个Python启动文件所示：

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 ROS2 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: 0
    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"

请注意以下几点：
- 对于以最大速度百分比设置速度限制的速度过滤器，成本地图过滤器的类型应设置为1。成本地图过滤器的所有可能类型可以在Map Server / Saver页面找到。
- 过滤器掩码话题名称应与成本地图过滤器信息发布者服务器的 mask_topic参数和地图服务器的topic_name参数相同。
- 如前一节所述，为了本教程的示例目的，系数base和multiplier应该分别设置为100.0和-1.0。
可以在navigation2_tutorials存储库的nav2_costmap_filters_demo目录中找到现成的独立Python启动脚本、带有ROS参数的YAML文件和用于速度过滤器的过滤器掩码示例。
为了简单地运行在“开始使用Nav2”教程中编写的Turtlebot3标准仿真上调好的过滤器信息发布者服务器和地图服务器节点，请构建该演示并执行如下所示命令来启动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

启用速度过滤器

成本地图过滤器是Costamp2D插件。
可以通过将speed_filter添加到nav2_params.yaml文件中的plugins参数来启用Costmap2D中的 SpeedFilter插件。 - -
SpeedFilter插件应该定义以下参数：
- plugin：插件类型，在本例中为nav2_costmap_2d::SpeedFilter。
- filter_info_topic：过滤信息话题名称。该参数要等于上面成本地图过滤器信息发布者服务器的filter_info_topic参数。
- speed_limit_topic：要发布速度限制到其上的话题名称
SpeedFilter插件支持的完整参数列表列在速度过滤器参数页面中。
可以将该插件放置在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"

正如其设计中所述，速度过滤器会发布针对控制器服务器的速度限制消息，以便控制器在需要时可以限制机器人的最大速度。控制器服务器有一个用于限制速度的ROS参数speed_limit_topic，该参数应该设置为与speed_filter插件中同名参数一样的值。
地图服务器中的这个话题还可用于除了速度限制区域之外的任意数量的其他速度限制应用程序，例如根据有效载荷质量动态调整最大速度。
将控制器服务器的speed_limit_topic参数设置为与为speed_filter插件中为该同名参数一样的值：

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

运行Nav2软件堆栈

在启动成本地图过滤器发布者服务器和地图服务器并为全局/局部成本地图启用速度过滤器后，按照“开始使用Nav2”教程中的说明运行Nav2软件堆栈，命令为：

ros2 launch nav2_bringup tb3_simulation_launch.py

为了更好地可视化速度过滤器掩码，在RViz左侧的Display窗格中展开map并将话题从/map更改为/speed_filter_mask。在限速区后面设置导航目标位置，并检查过滤器是否正常工作：机器人在通过限速区时应该减速。
如下图所示（第一张图片显示了为全局成本地图启用速度过滤器后的导航结果，第二张图片显示了speed_mask.pgm过滤器掩码图像）：

请输入图片描述

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号