使用禁区导航
概述
本教程将介绍如何简单地利用机器人无法进入的禁区/安全区以及在工业环境和仓库中移动的机器人的首选车道。所有这些功能都包含在“KeepoutFilter”成本地图过滤器插件中，该插件将在本文档中启用和使用。
要求
假设 ROS 2、Gazebo 和 TurtleBot3 软件包已在本地安装或构建。请确保 Nav2 项目也在本地构建，因为它是在 构建与安装 中创建的。
教程步骤
1. 准备过滤口罩
正如在 导航概念 中所写，任何 Costmap Filter（包括 Keepout Filter）都在读取 Filter Mask 文件中标记的数据。Filter Mask - 是通过 PGM、PNG 或 BMP 栅格文件分发的常见 Nav2 2D 地图，其元数据包含在 YAML 文件中。以下步骤有助于理解如何制作新的 Filter Mask：
创建一个 PGM/PNG/BMP 格式的新图像：将 turtlebot3_world.pgm 主地图（将在世界模拟中使用）从 Nav2 存储库复制到新的 keepout_mask.pgm 文件。
在您最喜欢的栅格图形编辑器中打开 keepout_mask.pgm``（例如，可以使用 GIMP 编辑器）。掩码上每个像素的亮度表示您将要使用的特定成本图过滤器的编码信息。每个像素的颜色亮度属于 ``[0..255] 范围（或百分比比例中的 [0..100]），其中 0 表示黑色，255 表示白色。另一个术语“暗度”将被理解为亮度的完全相反。换句话说， color_darkness = 100% - color_lightness。
在 GIMP 中，亮度通过颜色分量值表示（例如百分比比例中的 R），可以通过移动颜色更改工具中的 L 滑块来设置：
传入的掩码文件由地图服务器读取，并转换为 [0..100] 范围内的 OccupancyGrid 值（其中 0 ``表示空闲单元格， ``100 表示已占用，介于两者之间的任何值表示地图上占用的单元格较少或更多），或等于 -1 表示未知值。在 Nav2 堆栈中，每个地图都有 mode 属性，可以是 trinary 、 scale 或 raw 。根据所选的 mode ，PGM/PNG/BMP 的颜色亮度将通过以下原则之一转换为 OccupancyGrid ：
trinary(default mode): Darkness >=occupied_threshmeans that map occupied (100). Darkness <=free_thresh- map free (0). Anything in between - unknown status on map (-1).scale: Alpha <1.0- unknown. Darkness >=occupied_threshmeans that map occupied (100). Darkness <=free_thresh- map free (0). Anything in between - linearly interpolate to nearest integer from[0..100]range.raw: Lightness =0(dark color) means that map is free (0). Lightness =100(in absolute value) - map is occupied (100). Anything in between -OccupancyGridvalue = lightness. Lightness >=101- unknown (-1).
其中 free_thresh 和 occupied_thresh 阈值以最大亮度/暗度级别（ 255 ）的百分比表示。地图模式和阈值作为 mode 、 free_thresh 和 occupied_thresh 字段放置在 YAML 元数据文件（见下文）中。
Note
YAML 元数据文件中还有另一个参数，称为 negate。默认情况下，它设置为 false。当它设置为 true``时，较黑的像素将被视为空闲，较白的像素则被视为已占用。在这种情况下，对于 ``trinary 和 scale 模式，我们应该计算颜色亮度而不是暗度。 negate 对 raw 模式没有影响。
对于 Keepout Filter， OccupancyGrid``值与该单元格对应区域的通行能力成正比：值越高，不可通行区域越多。具有已占用值的单元格覆盖机器人永远不会进入或通过的禁入区。通过将 ``OccupancyGrid 设置为 [1-99] 之间的某个非占用值， KeepoutFilter 还可以充当 加权区域层 。机器人可以在这些区域内移动，但是它在那里出现是 不受欢迎的 （值越高，规划人员越早尝试将机器人移出该区域）。
禁止通行过滤器还涵盖首选车道的情况，在这种情况下，机器人只能在预定义的车道和允许区域内移动，例如在仓库中。要使用此功能，您需要准备遮罩图像，其中车道和允许区域将用自由值标记，而所有其他区域将被占用。在 三元 或 缩放 模式下绘制遮罩的提示：通常，属于车道的像素数量远少于覆盖其他区域的像素数量。在这种情况下，最初可以使用黑色铅笔在白色背景上绘制所有车道数据，然后（在保存 PGM 之前）可以使用图像光栅编辑器中的 颜色反转 工具。
为了简单起见，在示例中，用黑色填充要标记为禁区的区域（在 ``三元``模式下，这意味着占用地图）：
image: turtlebot3_world.pgm
->
image: keepout_mask.pgm
由于过滤器掩码图像是作为主图的副本创建的，因此 YAML 文件的其他字段无需更改。保存 keepout_mask.yaml ，新的过滤器掩码即可使用。
Note
世界地图本身和过滤器蒙版可能具有不同的大小、原点和分辨率，这可能很有用，例如当过滤器蒙版覆盖地图上较小的区域或一个过滤器蒙版被重复使用多次时（例如注释酒店中相同形状房间的禁区）。对于这种情况，您还需要更正 YAML 中的 分辨率 和 ``原点``字段，以便过滤器蒙版正确地放置在原始地图之上。
Note
另一个重要的注意事项是，由于 Costmap2D 不支持方向， origin 向量的最后第三个“偏航”分量应等于零。例如： origin: [1.25, -5.18, 0.0] 。
2. 配置 Costmap 过滤器信息发布服务器
每个 costmap 过滤器都会在 nav2_msgs/CostmapFilterInfo 类型的消息中读取传入的元信息（例如过滤器类型或数据转换系数）。这些消息由 Costmap Filter Info Publisher Server 发布。该服务器作为生命周期节点运行。根据 设计文档，nav2_msgs/CostmapFilterInfo 消息与 OccupancyGrid 过滤器掩码主题成对出现。因此，除了 Costmap Filter Info Publisher Server 之外，还应启用一个配置为发布过滤器掩码的 Map Server 新实例。
为了在您的配置中启用 Keepout 过滤器，应在 Python 启动文件中将两个服务器启用为生命周期节点。 也可以将它们作为组合节点添加到导航组件容器中，如下所示：
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.substitutions import NotEqualsSubstitution
from launch_ros.actions import Node, LoadComposableNodes
from launch_ros.actions import PushRosNamespace
from launch_ros.descriptions import ComposableNode
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')
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')
use_composition = LaunchConfiguration('use_composition')
container_name = LaunchConfiguration('container_name')
container_name_full = (namespace, '/', container_name)
# 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',
description='Full path to the ROS2 parameters file to use')
declare_mask_yaml_file_cmd = DeclareLaunchArgument(
'mask',
description='Full path to filter mask yaml file to load')
declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition', default_value='True',
description='Use composed bringup if True')
declare_container_name_cmd = DeclareLaunchArgument(
'container_name', default_value='nav2_container',
description='The name of container that nodes will load in if use composition')
# 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)
load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[
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]),
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]),
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}])
]
)
load_composable_nodes = GroupAction(
condition=IfCondition(use_composition),
actions=[
PushRosNamespace(
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
namespace=namespace),
LoadComposableNodes(
target_container=container_name_full,
composable_node_descriptions=[
ComposableNode(
package='nav2_map_server',
plugin='nav2_map_server::MapServer',
name='filter_mask_server',
parameters=[configured_params]),
ComposableNode(
package='nav2_map_server',
plugin='nav2_map_server::CostmapFilterInfoServer',
name='costmap_filter_info_server',
parameters=[configured_params]),
ComposableNode(
package='nav2_lifecycle_manager',
plugin='nav2_lifecycle_manager::LifecycleManager',
name='lifecycle_manager_costmap_filters',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': lifecycle_nodes}]),
]
)
]
)
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(declare_use_composition_cmd)
ld.add_action(declare_container_name_cmd)
ld.add_action(load_nodes)
ld.add_action(load_composable_nodes)
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: 0
filter_info_topic: "/costmap_filter_info"
mask_topic: "/keepout_filter_mask"
base: 0.0
multiplier: 1.0
filter_mask_server:
ros__parameters:
use_sim_time: true
frame_id: "map"
topic_name: "/keepout_filter_mask"
yaml_filename: "keepout_mask.yaml"
请注意：
对于 Keepout 过滤器，costmap 过滤器的
type应设置为0。过滤器掩码主题名称应等于 Costmap 过滤器信息发布服务器的
mask_topic参数和 Map Server 的topic_name参数。根据 Costmap 过滤器的设计，
OccupancyGrid值被线性转换为过滤器空间中的特征图。对于 Keepout 过滤器，这些值直接作为过滤器空间值传递，而无需进行线性转换。即使 Keepout 过滤器中不使用base和multiplier系数，也应将它们分别设置为0.0和1.0，以明确表明我们从OccupancyGrid值到过滤器值空间进行了一对一转换。
可以在 navigation2_tutorials 存储库的 nav2_costmap_filters_demo 目录中找到现成的独立 Python 启动脚本、带有 ROS 参数的 YAML 文件和 Keepout 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:=`pwd`/src/navigation2_tutorials/nav2_costmap_filters_demo/params/keepout_params.yaml mask:=`pwd`/src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yaml use_composition:=True
3. 启用禁止访问过滤器
Costmap Filters 是 Costamp2D 插件。您可以通过将 keepout_filter 添加到 nav2_params.yaml 中的 plugins 参数来启用 Costmap2D 中的 KeepoutFilter 插件。您可以将其放置在 global_costmap 中，以便使用 keepouts 和 local_costmap 进行规划，以确保机器人不会尝试穿过禁区。KeepoutFilter 插件应定义以下参数：
plugin: 插件类型。在我们的例子中是nav2_costmap_2d::KeepoutFilter.filter_info_topic: 过滤信息topic名称。这需要等于上一章中Costmap Filter Info Publisher Server的``filter_info_topic``参数。
KeepoutFilter 支持的参数完整列表列于：ref: keepout_filter 页面。
值得注意的是，仅为“global_costmap”启用“KeepoutFilter”将导致路径规划器构建绕过禁区规划。仅为“local_costmap”启用“KeepoutFilter”将导致机器人不会进入禁区，但路径仍可能穿过它们。因此，最佳做法是同时为全局和局部代价地图启用“KeepoutFilter”，方法是将其添加到“nav2_params.yaml”中的“global_costmap”和“local_costmap”中。但这并不总是正确的。在某些情况下，全局和局部代价地图的禁区不必相同，例如，如果机器人不允许故意进入禁区，但如果禁区在那里，机器人可以在碰到边缘或角落时快速进出。对于这种情况，不需要使用本地代价地图副本的额外资源。
要对全局和局部代价地图启用具有相同掩码的 KeepoutFilter ，请使用以下配置：
global_costmap:
global_costmap:
ros__parameters:
...
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
filters: ["keepout_filter"]
...
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
enabled: True
filter_info_topic: "/costmap_filter_info"
...
local_costmap:
local_costmap:
ros__parameters:
...
plugins: ["voxel_layer", "inflation_layer"]
filters: ["keepout_filter"]
...
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
enabled: True
filter_info_topic: "/costmap_filter_info"
Note
所有代价地图过滤器都应通过 filters 参数启用——尽管从技术上讲，可以将其包含在分层代价地图本身中。这与图层插件分开，以防止对图层（尤其是膨胀图层）的干扰。
4. 运行 Nav2 堆栈
启动 Costmap Filter Info Publisher Server 和 Map Server 并为全局/局部代价地图启用 Keepout Filter 后，按照:ref:getting_started 中的说明运行 Nav2 堆栈：
ros2 launch nav2_bringup tb3_simulation_launch.py
并检查过滤器是否正常工作，如下图所示（第一张图片显示为全局代价地图启用的keepout过滤器，第二张图片显示不同大小的 keepout_mask.pgm 过滤器掩码）：
