ROS2与Navigation2入门教程-使用禁区进行导航
说明：
- 介绍使用禁区进行导航
概述
本教程将会说明如何简单地利用机器人不能进入的禁区/安全区以及机器人在工业环境和仓库中移动的首选车道。
所有这些功能都由 KeepoutFilter成本地图过滤器插件提供，在本教程中将会启用和使用该插件。
要求
- 教程假设已经在本地机器上安装或者构建好了ROS 2、Gazebo和TurtleBot3软件包
具体步骤
准备过滤器掩码
正如“移动机器人导航概念”教程中所述，任何成本地图过滤器（包括 Keepout过滤器）都是在读取过滤器掩码文件中标记的数据。过滤器掩码就是通常的Nav2 2D地图，这种地图通过PGM、PNG或 BMP栅格文件分发，其元数据则包含在一个YAML文件中。下面的步骤有助于了解如何制作新的过滤器掩码：
首先，新建一个PGM/PNG/BMP格式的图像：将用于机器人世界仿真的turtlebot3_world.pgm主地图从Nav2存储库中复制到一个新的keepout_mask.pgm文件。
其次，在您最喜欢使用的栅格栅图形编辑器软件（例如GIMP 编辑器）中打开keepout_mask.pgm文件。掩码上每个像素的亮度（lightness）表示您将要使用的特定成本地图过滤器的编码信息。每个像素的颜色亮度值范围为[0..255]（或以百分比格式的[0..100]），其中0表示黑色，255 表示白色。另一个术语“暗度（darkness）”可以理解为与亮度恰好相反的概念。换句话说，color_darkness = 100%等同于color_lightness = 0%。
在GIMP软件中，亮度是通过颜色分量值（例如以百分比表示的 R）表示的，并且可以通过移动颜色更改工具中的L滑块来设置：
然后地图服务器会读取传入的掩码文件，并将其转换为[0..100]值域范围的OccupancyGrid值（其中0表示未被占用的或空闲的单元格，100表示已被占用的单元格，介于两者之间的值则表示地图上或多或少被占用的单元格），或者等于-1则表示该单元格的占用情况未知。
在Nav2软件堆栈中，每张地图都有mode属性，该属性可以是trinary、scale或者raw之一。
取决于所选择的mode，PGM/PNG/BMP的颜色亮度会用以下原则之一转换为OccupancyGrid：
trinary模式（默认模式）：暗度（darkness） >= occupied_thresh表示地图被占用（100）。暗度 <= free_thresh 表示地图未被占用或是空闲的（0）。暗度介于两者之间的任何情况表示地图上单元格占用情况的未知状态（-1）。
scale模式：Alpha < 1.0 表示占用情况未知。暗度 >= occupied_thresh表示地图被占用（100）。暗度 <= free_thresh 表示地图未被占用或是空闲的（0）。暗度介于两者之间的任何情况表示从[0..100] 范围线性插值到最接近的整数。
raw模式：亮度（Lightness） = 0（暗色）表示地图未被占有或是空闲的（0）。亮度 = 100（绝对值）表示地图被占用（100）。亮度介于两者之间的任何值表示OccupancyGrid 值 = 亮度。亮度 >= 101则表示地图单元格占用情况未知（-1）。
其中free_thresh和occupied_thresh这两个阈值被表示为百分比形式的最大亮度/暗度级别（255）。
地图模式和这两个阈值可以分别用YAML元数据文件中的mode、free_thresh和occupied_thresh字段设置（见下文）。
YAML元数据文件中有另一个称为 negate的参数。默认情况下，该参数设置为false。当该参数设置为true时，较暗的像素将被视为空闲像素，而较白的像素将被视为已占用。
在negate参数设置为true的情况下，应该为trinary和scale模式计算颜色亮度而不是暗度。Negate参数对raw模式没有影响
对于禁区（Keepout）过滤器，OccupancyGrid的值与单元格对应区域的可通行性成比例关系：较高的值意味着更多不可通行的区域。
具有占用值的单元格覆盖的区域是机器人永远不会进入或通过的禁区。
KeepoutFilter还可以通过将OccupancyGrid设置为[1-99]非占用值之间的值来充当“加权区域图层”。
机器人允许在这些区域内移动，但是机器人在那里出现将会是“不受欢迎的”（值越高，规划器就会越早尝试将机器人从该区域中移出来）。
禁区（Keepout）过滤器还涉及首选车道用例，即例如在仓库用例中，机器人应该仅在预定义的车道和允许的区域内移动。要使用这项功能，需要准备掩码图像，在掩码图像中车道和允许区域将标记有空闲值，而所有其他区域会被标记为占用。对于在trinary或scale模式情况下绘制掩码的提示：通常，属于车道的像素数量远少于属于其他区域的像素。在这种情况下，最初可能要使用黑色铅笔在白色背景上绘制所有车道数据，然后（就在保存PGM之前）可能会要使用图像栅格编辑器中的“颜色反转”工具。
为简单起见，在本示例中会用黑色填充这些区域（在trinary模式下这表示地图已被占用的区域），这些区域将会被标记为禁区：
在所有禁区都被填充后保存keepout_mask.pgm图像。
与所有其他地图一样，过滤器掩码应该有自己的YAML元数据文件。将turtlebot3_world.yaml复制到keepout_mask.yaml文件。
打开 keepout_mask.yaml文件，并将image字段更正为新制作的PGM掩码：
image: turtlebot3_world.pgm
->
image: keepout_mask.pgm
由于过滤器掩码图像是作为主地图的副本创建的，因此不需要更改YAML文件的其他字段。
保存keepout_mask.yaml文件，这样新的过滤器掩码就准备就绪可以使用了。
机器人世界地图本身和过滤器掩码可能具有不同的尺寸、原点和分辨率，这可能会很有用，例如当过滤器掩码覆盖地图上较小区域时的情况，或者当多次重复使用同一个过滤器掩码的情况（例如为酒店中相同形状的房间注释禁区）。
对于这种情况，还需要更正 YAML文件中的resolution和origin字段，以便过滤器掩码能够正确地放置在原始地图的上面。
另一个重要的注意事项就是，由于Costmap2D不支持方向，origin向量的最后第三个分量“偏航角（yaw）”应该等于零。
例如：
origin：[1.25, -5.18, 0.0]。
配置成本地图过滤器信息发布者服务器
每个成本地图过滤器都会在一条nav2_msgs/CostmapFilterInfo类型的消息中读取传入的元信息（例如过滤器类型或数据转换系数）。 这些消息会由成本地图过滤器信息发布者服务器（Costmap Filter Info Publisher Server）发布。
该服务器是作为一个生命周期节点来运行的。 根据其设计文档，nav2_msgs/CostmapFilterInfo消息与OccupancyGrid过滤器掩码话题会成对出现。
因此，在启用成本地图过滤器信息发布者服务器的同时，还应该启用一个新的被配置成发布过滤器掩码的地图服务器（Map Server）实例。
为了在配置中启用禁区（Keepout）过滤器，这两个服务器都应该在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', 'keepout_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', 'keepout_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: "/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"
请注意以下几点：
对于禁区过滤器，成本地图过滤器的类型应该设置为0。
过滤器掩码话题名称应与成本地图过滤器信息发布者服务器的 mask_topic参数和地图服务器的topic_name参数相同。
根据成本地图过滤器的设计，OccupancyGrid值会被线性转换为过滤器空间中的特征图。对于禁区过滤器，这些值会直接作为过滤器空间值被传递，而无需进行线性转换。即使在禁区过滤器中未使用base和multiplier系数，这两个系数也应该相应地设置为0.0和1.0，以便明确显示从OccupancyGrid值转换到过滤器值空间的一对一转换。
可以在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/keepout_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yaml
启用禁区过滤器
成本地图过滤器是Costamp2D插件。可以通过将keepout_filter添加到nav2_params.yaml文件中的plugins参数来启用Costmap2D中的 KeepoutFilter插件。
可以将其放置在global_costmap中以使用禁区和local_costmap进行路径规划，以确保机器人不会尝试驶过禁区。KeepoutFilter插件应该定义以下参数：
plugin：插件类型，在本例中为nav2_costmap_2d::KeepoutFilter。
filter_info_topic：过滤信息话题名称。该参数要等于上面成本地图过滤器信息发布者服务器的filter_info_topic参数。
KeepoutFilter插件支持的完整参数列表列在禁区过滤器参数页面中。
请务必注意，仅为global_costmap启用KeepoutFilter插件将会导致路径规划器绕过禁区来构建路径规划。
仅为local_costmap启用KeepoutFilter插件将会导致机器人无法进入禁区，但路径可能仍会通过这些禁区。
因此，最好的做法就是通过将它添加到nav2_params.yaml文件中的global_costmap和local_costmap来同时为全局和局部成本地图启用KeepoutFilter插件。然而，这并不总是正确的。
v在某些情况下，全局和局部成本地图的禁区不必相同，例如如果机器人不允许故意进入禁区，但如果机器人在禁区中，如果机器人夹在边缘或角落，它可以非常快地驶入和驶出。对于这种情况，不需要使用局部成本地图副本这种额外资源。
要为全局和局部成本图启用具有相同掩码的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"
所有成本地图过滤器都应该通过一个filters参数来启用，尽管从技术上来说可以包含在分层的成本地图本身中。
这会与图层插件分开，以防止图层中尤其是膨胀层中的干扰。
运行Nav2软件堆栈
- 在启动成本地图过滤器发布者服务器和地图服务器并为全局/局部成本地图启用禁区过滤器后，按照“开始使用Nav2”教程中的说明运行Nav2软件堆栈，命令为：
ros2 launch nav2_bringup tb3_simulation_launch.py
- 并检查该过滤器是否正常工作，如下图所示（第一张图片显示了为全局成本地图启用禁区过滤器后的导航结果，第二张图片显示了不同尺寸的 keepout_mask.pgm过滤器掩码）：
参考：
https://navigation.ros.org/tutorials/docs/navigation2_with_keepout_filter.html
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号