带速度限制的导航
概述
本教程演示了如何简单地使用Speed Filter，该过滤器旨在限制地图上标记的速度限制区域中机器人的最大速度。这个功能由``SpeedFilter``成本地图过滤器插件实现，本文档将启用并使用该插件。
教程步骤
1. 准备过滤掩模
如在 导航概念 中所述，任何成本地图过滤器（包括Speed Filter）都会读取过滤器遮罩文件中标记的数据。有关过滤器遮罩、它们的类型、详细结构以及如何创建新的遮罩的所有信息都在 使用禁止区域进行导航 教程的 1. 准备过滤器遮罩 章节中有说明。绘制Speed Filter的过滤器遮罩的原则与Keepout Filter相同（在地图上注释所请求的区域），只是``OccupancyGrid`` 遮罩值具有另一层含义：这些值是对应于地图上的单元格的区域的编码速度限制。
让我们来看看它是如何解码的。正如我们所知，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为单位）。
Speed Filter使用的含义从 nav2_msgs/CostmapFilterInfo 消息中读取。在本教程中，我们将使用以最大机器人速度的百分比表示的第一种类型的速度限制。
注解
对于以百分比表示的速度限制，speed_limit 将准确地使用百分比，属于 [0..100] 范围，而不是 [0.0..1.0] 范围。
使用PGM/PNG/BMP格式创建一个新的图像：从Nav2存储库中复制用于世界仿真的 turtlebot3_world.pgm 主地图，并保存到一个名为 speed_mask.pgm 的新文件中。在您喜欢的光栅图形编辑器中打开 speed_mask.pgm，并用灰色填充速度受限区域。在我们的示例中，较深的颜色表示具有更高速度限制的区域：
区域 "A" 填充了 40% 的灰色，区域 "B" 填充了 70% 的灰色，这意味着速度限制将分别在区域 "A" 中占 100% - 40% = 60%，在区域 "B" 中占 100% - 70% = 30%，这是机器人允许的最大速度值。我们将使用没有阈值的 scale 地图模式。在这种模式下，较暗的颜色将具有较高的 OccupancyGrid 值。例如，在具有 70% 灰度的区域 "B" 中，OccupancyGrid 数据将等于 70。因此，为了达到目标，我们需要选择 base = 100.0 和 multiplier = -1.0。这将反转比例尺的 OccupancyGrid 值为所需值。为了方便起见，在 yaml 文件中选择不设阈值（free_thresh、occupied_thresh），以实现从过滤器蒙版的亮度值到速度限制百分比的完整转换。
注解
这是一种典型但非强制的选择，可以选择``base``和``multiplier``。例如，您可以选择``map``模式为``raw``。在这种情况下，颜色亮度直接转换为``OccupancyGrid``值。对于以``raw``模式保存的掩码，base``和``multiplier``将分别等于``0.0``和``1.0。
另一个重要的事情是，不必使用整个 [0..100] 百分比范围。可以选择 base 和 multiplier 系数，使速度限制值位于百分比范围的中间某处。例如，base = 40.0，multiplier = 0.1 将以步长为 0.1% 的速度限制值从 [40.0%..50.0%] 范围内获得。这对于精细调整可
在填充完所有的速度限制区域之后，保存``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 字段，以便将过滤器掩码正确放置在原始地图上。本示例显示了使用主地图作为基础，但这不是必需的。
2. 配置成本地图过滤器信息发布服务器
每个costmap过滤器通过``nav2_msgs/CostmapFilterInfo``类型的消息读取传入的元信息（例如过滤器类型或数据转换系数）。这些消息由`Costmap Filter Info Publisher Server <https://github.com/ros-planning/navigation2/tree/main/nav2_map_server/src/costmap_filter_info>`_发布。该服务器作为生命周期节点运行。根据设计文档<https://github.com/ros-planning/navigation2/blob/main/doc/design/CostmapFilters_design.pdf>，``nav2_msgs/CostmapFilterInfo``消息与``OccupancyGrid``过滤器掩码主题成对出现。因此，除了Costmap Filter Info Publisher Server外，还应启用新的Map Server实例，配置为发布过滤器掩码。
为了在配置中启用速度过滤器，两个服务器都应该在Python launch文件中作为生命周期节点启用。例如，可以如下所示：
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``变
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"
请注意：
对于Speed Filter中以最大速度的百分比设置速度限制，costmap过滤器的``type``应设置为``1``。可在:ref:`configuring_map_server`页面找到所有可能的costmap过滤器类型。
过滤器掩码主题名称应与成本地图过滤器信息发布服务器的``mask_topic``参数和地图服务器的``topic_name``参数相等。
如前一章所述，为了本教程示例的目的，
base和multiplier应该分别设置为100.0和-1.0。
可在``navigation2_tutorials``存储库的`nav2_costmap_filters_demo <https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_costmap_filters_demo>`_目录中找到使用示例的独立的Python启动脚本、带有ROS参数的YAML文件和Speed Filter的过滤器掩码。要简单运行Filter Info Publisher Server和调整为在:ref:getting_started`中编写的Turtlebot3标准仿真上运行的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插件。要在Costmap2D中启用``SpeedFilter``插件，需要将``speed_filter``添加到``nav2_params.yaml``中的``plugins``参数中。Speed Filter插件应定义以下参数：
plugin: 插件的类型。在我们的例子中是nav2_costmap_2d::SpeedFilter。filter_info_topic: 过滤器信息主题名称。这个值需要与上面章节中的 Costmap Filter Info Publisher Server 的filter_info_topic参数相等。speed_limit_topic: 发布速度限制的主题名称。
SpeedFilter 支持的参数完整列表在 速度过滤器参数 页面中列出。
您可以将插件放置在``nav2_params.yaml``文件中的``global_costmap``部分，以将速度限制蒙版应用于全局代价图，或者放置在``local_costmap``中，以将速度蒙版应用于局部代价图。然而，``SpeedFilter``插件不应同时启用全局和局部代价图。否则，可能会导致在速度限制边界上产生不需要的多个"速度限制" - "无限制"消息链，从而导致机器人的抖动或其他不可预测的行为。
在本教程中，我们将为全局代价地图启用 Speed Filter。请使用以下配置：
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>`_中所述，Speed Filter发布用于控制器服务器的速度限制`消息 <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg>`_，以便在需要时限制机器人的最大速度。控制器服务器有一个名为``speed_limit_topic``的ROS参数，它应设置为与``speed_filter``插件值相同。该话题在地图服务器中还可用于除速度限制区域之外的任意数量的其他受速度限制的应用程序，例如根据负载质量动态调整最大速度。
将 Controller Server 的 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 中，在左侧的 Displays 面板中展开 Map 并将 Topic 从 /map 更改为 /speed_filter_mask。设置目标在速度限制区域后面，并检查过滤器是否正常工作：机器人在通过速度限制区域时应该减速。下面是可能的效果（第一张图片显示了全局代价地图启用了速度过滤器，第二张图片是 speed_mask.pgm 过滤器遮罩）：
