使用矢量对象进行导航（Vector Objects）

概述
环境要求
配置矢量对象服务器
准备 Nav2 软件栈
演示运行
使用矢量对象

矢量对象演示

概述

本教程展示如何在栅格代价地图（raster costmap）上添加矢量对象（Vector Object）并据此导航。矢量对象有多种用途，例如通过编程方式生成完整的占用栅格地图用于导航、在现有地图上添加虚拟的静态/动态障碍物，或者像 Costmap 过滤器（Costmap Filters）对栅格化地图所做的那样——只不过是在矢量（多边形或形状）的基础上进行，而不是标注一张与地图等大的掩码（mask）。

在本教程中，添加的矢量对象会通过禁区过滤器（Keepout Filter）作为代价地图中的障碍物。为此，需要准备启用了禁区过滤器的 Nav2 软件栈，并同时运行矢量对象服务器（Vector Object Server）。矢量对象服务器会发布一张 OccupancyGrid 地图，将栅格化后的矢量对象作为禁区过滤器的输入掩码。其他用例的原理类似，完成本教程后稍作修改即可适配。

注意：禁区过滤器适合用于添加虚拟障碍物或在代价地图中排除某些区域，但矢量对象服务器的应用不止于此。它可以与其他 Costmap 过滤器搭配使用，甚至完全用于其他场景，例如标记多边形速度限制区域、用二进制过滤器（Binary Filter）定义一个多边形房间并在其中关闭摄像头，或用于自定义的空间/多边形应用。

环境要求

假定 ROS 2 和 Nav2 依赖包已安装或在本地构建。请按照构建说明中的说明操作。为了更好地理解禁区过滤器（Keepout Filter）的工作原理（它是当前配置的一部分），还建议先通读使用禁区（Keepout Zones）导航教程。

配置矢量对象服务器

矢量对象服务器（Vector Object Server）自带演示用的 vector_object_server.launch.py 启动文件和 vector_object_server_params.yaml 预置参数文件，将其集成到 Nav2 的主启动文件中也很简单。

本教程重点介绍如何利用这一简单配置在代价地图上添加虚拟障碍物。出于演示目的，我们指定两个障碍物形状：一个三角形多边形和一个以「occupied」（占用）值填充的圆形，以防止机器人穿过它们。多边形和圆形的 YAML 配置如下：

shapes: ["Poly", "Circle"]
Poly:
  type: "polygon"
  frame_id: "map"
  closed: True
  value: 100
  points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2]
Circle:
  type: "circle"
  frame_id: "map"
  fill: True
  value: 100
  center: [1.5, 0.5]
  radius: 0.7

其中，三角形多边形由 {0.3, 0.5}、{-0.4, 1.2}、{-0.4, -0.2} 这 3 个点指定；圆形的圆心坐标为 {1.5, 0.5}、半径为 0.7 米。两者所在的 frame 均为 map。多边形的 closed 值为 true、圆形的 fill 值为 true，表示两个形状都将以指定的 value 值填充。该值为 100，在 OccupancyGrid 格式中表示「occupied」（占用）。

注意：矢量对象的 frame 指定为与地图的全局 frame 相同，这样便于在地图上放置静态对象。当然，也可以在不同于全局地图 frame 的坐标系中指定形状。此时，矢量对象服务器会以动态方式处理和发布输出地图，适用于移动对象。

注意：每个形状通过 UUID 标识，UUID 可以以字符串格式手动指定。演示中我们没有在参数中指定形状的 UUID，矢量对象服务器会自动为每个形状生成新的 UUID。之后可通过调用 GetShapes.srv 服务获取 UUID 列表。

Costmap 过滤器要求 CostmapFilterInfo.msg 消息与过滤器掩码（包含矢量形状的栅格化地图）一起发布。 Costmap Filter Info 消息由 Costmap Filter Info 服务器发布，该服务器同样通过 vector_object_server.launch.py 脚本启动。

用于演示的完整 vector_object_server_params.yaml YAML 文件如下所示：

vector_object_server:
  ros__parameters:
    map_topic: "vo_map"
    global_frame_id: "map"
    resolution: 0.05
    default_value: -1
    overlay_type: 0
    update_frequency: 1.0
    transform_tolerance: 0.1
    shapes: ["Poly", "Circle"]
    Poly:
      type: "polygon"
      frame_id: "map"
      closed: True
      value: 100
      points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2]
    Circle:
      type: "circle"
      frame_id: "map"
      fill: True
      value: 100
      center: [1.5, 0.5]
      radius: 0.7
costmap_filter_info_server:
  ros__parameters:
    type: 0
    filter_info_topic: "vo_costmap_filter_info"
    mask_topic: "vo_map"
    base: 0.0
    multiplier: 1.0

关于矢量对象服务器各参数及其工作原理的详细信息，可参阅矢量对象服务器配置配置指南。 Costmap Filter Info 服务器参数的说明可在Costmap Filter Info 服务器配置页面找到。

在矢量对象服务器和 Costmap Filter Info 服务器配置完成后，通过以下命令启动它们：

ros2 launch nav2_map_server vector_object_server.launch.py

准备 Nav2 软件栈

矢量对象服务器将形状写入 OccupancyGrid 地图，并通过话题发布，作为 Nav2 中禁区过滤器的输入掩码。在 Nav2 软件栈中启用禁区过滤器的原理与使用禁区（Keepout Zones）导航教程中描述的类似。由于矢量对象是在全局代价地图中启用的，因此需要在标准 Nav2 配置 nav2_params.yaml 的全局代价地图部分添加名为 vector_object_layer 的禁区过滤器，如下所示：

global_costmap:
  ros__parameters:
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
    filters: ["keepout_filter", "speed_filter", "vector_object_layer"]
    ...
    vector_object_layer:
      plugin: "nav2_costmap_2d::KeepoutFilter"
      enabled: True
      filter_info_topic: "vo_costmap_filter_info"

演示运行

在矢量对象服务器启动、全局代价地图启用矢量对象层后，按照快速入门中的说明运行 Nav2 软件栈：

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

这里使用了可组合节点（composable nodes）技术，因此运行矢量对象服务器的控制台应出现以下消息：

[leha@leha-PC nav2_ws]$ ros2 launch nav2_map_server vector_object_server.launch.py
[INFO] [launch]: All log files can be found below /home/leha/.ros/log/2023-11-24-13-18-42-257011-leha-PC-18207
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/lifecycle_manager_vo_server' in container 'nav2_container'
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/vector_object_server' in container 'nav2_container'
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/costmap_filter_info_server' in container 'nav2_container'

最后几行表示所有三个节点——矢量对象服务器、Costmap Filter Info 服务器以及管理它们的生命周期管理器（Lifecycle Manager）——都已成功加载到 Nav2 容器 nav2_container 中。

为机器人设置初始位姿，并检查矢量对象是否已出现在全局代价地图上：

矢量对象显示在代价地图上

与禁区过滤器类似，机器人会将矢量对象视为代价地图上的障碍物并自动避开：

矢量对象避障

使用矢量对象

运行过程中，矢量对象可以被修改、添加或移除。下面我们将三角形形状改为横条（bar）。

如前所述，每个形状都由其自身的 UUID 管理；如果在参数中没有显式指定，矢量对象服务器会自动生成。要获取形状的 UUID，请在控制台中调用 GetShapes.srv 服务：

ros2 service call /vector_object_server/get_shapes nav2_msgs/srv/GetShapes

预期输出如下：

requester: making request: nav2_msgs.srv.GetShapes_Request()

response:
nav2_msgs.srv.GetShapes_Response(circles=[nav2_msgs.msg.CircleObject(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='map'), uuid=unique_identifier_msgs.msg.UUID(uuid=array([73, 141, 241, 249, 116, 24, 69, 81, 178, 153, 159, 19, 245, 152, 28, 29], dtype=uint8)), center=geometry_msgs.msg.Point32(x=1.5, y=0.5, z=0.0), radius=0.699999988079071, fill=True, value=100)], polygons=[nav2_msgs.msg.PolygonObject(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='map'), uuid=unique_identifier_msgs.msg.UUID(uuid=array([153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39], dtype=uint8)), points=[geometry_msgs.msg.Point32(x=0.30000001192092896, y=0.5, z=0.0), geometry_msgs.msg.Point32(x=-0.4000000059604645, y=1.2000000476837158, z=0.0), geometry_msgs.msg.Point32(x=-0.4000000059604645, y=-0.20000000298023224, z=0.0)], closed=True, value=100)])

在我们的例子中，三角形多边形的 UUID 为 [153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39]。

调用 AddShapes.srv 服务时，如果没有指定 UUID 或给定的 UUID 未找到，将添加新形状；如果 UUID 已存在，则更新该形状。现在我们要把三角形改为由 4 个点组成的横条多边形。使用获取到的 UUID 调用以下服务请求即可完成：

ros2 service call /vector_object_server/add_shapes nav2_msgs/srv/AddShapes "polygons: [{points: [{x: 0.0, y: 1.0}, {x: -0.5, y: 1.0}, {x: -0.5, y: 0.0}, {x: 0.0, y: 0.0}], closed: true, value: 100, uuid: {uuid: [153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39]}}]"

矢量对象服务器中的多边形形状随即被更新，vo_map 也会刷新，得到的代价地图如下所示——三角形障碍物已更新为横条：

矢量对象已更新

最后，从地图上移除所有形状。要移除现有形状，可使用 RemoveShapes.srv 服务。该服务接收 UUID 数组来指定要移除的形状，也可以使用 all_objects 选项一次性移除所有形状。这里我们采用后者：

ros2 service call /vector_object_server/remove_shapes nav2_msgs/srv/RemoveShapes "all_objects: true"

结果，所有矢量形状从全局代价地图上消失：

矢量对象已移除