costmap_2d（1）

costmap_2d

1.简介

这个包提供了一个 2D 代价地图的实现，它从世界上获取传感器数据，构建数据的 2D 或 3D 占用网格（取决于是否使用基于体素的实现），并在基于占用网格和用户指定的膨胀半径。该软件包还支持基于 map_server 的代价地图初始化、基于滚动窗口的代价地图以及基于参数的传感器主题订阅和配置。

注：上图中，红色方格代表costmap中的障碍物，蓝色方格代表被机器人内切半径膨胀的障碍物，红色多边形代表机器人的足迹。为了使机器人避免碰撞，机器人的足迹不应与红色单元格相交，机器人的中心点不应与蓝色单元格相交。

costmap_2d 包提供了一个可配置的结构，该结构以占用网格的形式维护有关机器人应该导航到哪里的信息。成本地图使用来自静态地图的传感器数据和信息，通过 costmap_2d::Costmap2DROS 对象存储和更新关于世界障碍物的信息。 costmap_2d::Costmap2DROS 对象为其用户提供纯二维界面，这意味着关于障碍物的查询只能在列中进行。例如，一张桌子和一只鞋在 XY 平面中的相同位置，但具有不同的 Z 位置将导致 costmap_2d::Costmap2DROS 对象的成本图中的相应单元格具有相同的成本值。这是为了帮助规划平面空间。

从 Hydro 版本开始，用于将数据写入代价地图的底层方法是完全可配置的。每一点功能都存在于一个层中。比如静态地图是一层，障碍物是另一层。默认情况下，障碍层维护三维信息（请参阅 voxel_grid）。维护 3D 障碍物数据可以让图层更智能地处理标记和清除。

主要接口是 costmap_2d::Costmap2DROS，它保留了大部分与 ROS 相关的功能。它包含一个 costmap_2d::LayeredCostmap，用于跟踪每个层。每个层都使用 pluginlib 在 Costmap2DROS 中实例化，并添加到 LayeredCostmap。这些层本身可以单独编译，允许通过 C++ 接口对成本图进行任意更改。 costmap_2d::Costmap2D 类实现了用于存储和访问二维成本图的基本数据结构。

下面描述了有关 Costmap 如何更新占用网格的详细信息，以及指向描述各个层如何工作的单独页面的链接。

2.标记和清除

成本图自动通过 ROS 订阅传感器主题并相应地更新自身。每个传感器用于标记（将障碍物信息插入成本图中）、清除（从成本图中删除障碍物信息）或两者。标记操作只是数组的索引，用于更改单元格的成本。然而，清除操作包括对报告的每个观察结果从传感器原点向外穿过网格进行光线追踪。如果使用三维结构来存储障碍物信息，则在将每一列的障碍物信息放入成本图中时，会将其向下投影到二维中。

3.占用空间、空闲空间和未知空间

虽然成本图中的每个单元格都可以具有 255 个不同的成本值之一（请参阅膨胀部分），但它使用的底层结构只能表示三个。具体来说，此结构中的每个单元格可以是空闲的、已占用的或未知的。在投影到成本图中时，每个状态都有一个分配给它的特殊成本值。具有一定数量的已占用单元格的列（参见 mark_threshold 参数）分配一个 costmap_2d::LETHAL_OBSTACLE 成本，具有一定数量的未知单元格的列（参见 unknown_threshold 参数）分配一个 costmap_2d::NO_INFORMATION 成本，其他列是分配了一个 costmap_2d::FREE_SPACE 成本。

4.地图更新

成本地图以 update_frequency 参数指定的速率执行地图更新周期。每个周期，传感器数据进来，标记和清除操作在成本图的底层占用结构中执行，并且该结构被投影到成本图中，其中如上所述分配适当的成本值。在此之后，每个障碍物膨胀都会在每个单元格上执行，成本为 costmap_2d::LETHAL_OBSTACLE。这包括将成本值从每个占用的单元格向外传播到用户指定的膨胀半径。下面概述了这个通货膨胀过程的细节。

5.tf

为了将来自传感器源的数据插入成本图中，costmap_2d::Costmap2DROS 对象广泛使用了 tf.具体来说，它假设由 global_frame 参数、robot_base_frame 参数和传感器源指定的坐标系之间的所有变换都是连接的并且是最新的。 transform_tolerance 参数设置这些转换之间允许的最大延迟量。如果 tf 树没有以这个预期的速度更新，导航堆栈就会停止机器人。

6.膨胀

膨胀是从随距离减小的占用单元中传播成本值的过程。为此，我们为与机器人相关的成本图值定义了 5 个特定符号。

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。
- 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
- 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
- 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
- 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
- 自由区域:栅格值为0，此处机器人可以自由通过；
- 未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

我们使用术语“可能”是因为它可能不是真正的障碍单元，而是一些用户偏好，将特定成本值放入地图中。例如，如果用户想要表达机器人应该尝试避开建筑物的特定区域，他们可以将自己的成本插入到该区域的成本图中，而不受任何障碍物的影响。请注意，虽然在上图中使用值为 128 作为示例，但真实值受代码中定义的 inscribed_radius 和 inflation_radius 参数的影响。

“未知”成本意味着没有关于给定单元格的信息。代价地图的用户可以按照他们认为合适的方式解释这一点。

根据它们与“致命”单元格的距离和用户提供的衰减函数，所有其他成本都被分配了一个介于“自由空间”和“可能受限”之间的值。

这些定义背后的基本原理是，我们将其留给规划者实施来关心或不关心确切的足迹，但给他们足够的信息，以便他们可以仅在方向真正重要的情况下承担追踪足迹的成本。

7.地图类型

初始化一个costmap_2d::Costmap2DROS对象主要有两种方式。

第一种是使用用户生成的静态地图作为种子（有关构建地图的文档，请参阅 map_server 包）。在这种情况下，代价地图被初始化以匹配静态地图提供的宽度、高度和障碍物信息。此配置通常与定位系统（如 amcl）结合使用，该系统允许机器人在地图框中注册障碍物，并在机器人驶过其环境时根据传感器数据更新其代价地图。

第二种初始化 costmap_2d::Costmap2DROS 对象的方法是给它一个宽度和高度，并将 rolling_window 参数设置为 true。 rolling_window 参数使机器人在世界各地移动时保持在代价地图的中心，当机器人移动到距离给定区域太远时，从地图中删除障碍物信息。这种类型的配置最常用于机器人只关心局部区域内障碍物的里程坐标系中。

8.Component API

8.1Costmap2DROS

costmap_2d::Costmap2DROS 对象是 costmap_2d::Costmap2D 对象的包装器，将其功能公开为 C++ ROS 包装器。它在初始化时指定的 ROS 名称空间（假定为此处的名称）内运行。

指定 my_costmap 命名空间的 costmap_2d::Costmap2DROS 对象的创建示例：

#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>

...

tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS costmap("my_costmap", tf);

如果您直接 rosrun 或 roslaunch costmap_2d 节点，它将在 costmap 命名空间中运行。在这种情况下，下面所有对 name 的引用都应该替换为 costmap。

更常见的情况是通过启动 move_base 节点来运行完整的导航堆栈。这将创建 2 个代价映射，每个都有自己的命名空间：local_costmap 和 global_costmap。您可能需要将某些参数设置两次，每个代价地图一次。

8.1.1 ROS API

从 Hydro 开始，C++ API 发生了变化。

8.1.2 Subscribed Topics

~<name>/footprint (geometry_msgs/Polygon)

机器人足迹规范。这取代了先前的封装参数规范。

8.1.3 Published Topics发表主题

~<name>/costmap (nav_msgs/OccupancyGrid)

成本图中的值

~<name>/costmap_updates

(map_msgs/OccupancyGridUpdate)

costmap更新区域的值

~<name>/voxel_grid

(costmap_2d/VoxelGrid)

当底层占用网格使用体素并且用户请求发布体素网格时，可以选择进行发布。

8.1.4 参数

Hydro 和更高版本对所有 costmap_2d 层使用插件。如果您不提供插件参数，那么初始化代码将假定您的配置是 Hydro 之前的配置，并将加载一组具有默认名称空间的默认插件。您的参数将自动移动到新的命名空间。默认命名空间是 static_layer、obstacle_layer 和 inflation_layer。一些教程（和书籍）仍然引用 Hydro 之前的参数，因此请密切注意。为了安全起见，请务必提供一个插件参数。

~<name>/plugins (sequence, default: pre-Hydro behavior)

插件规范的顺序，每层一个。每个规范都是一个包含名称和类型字段的字典。该名称用于定义插件的参数命名空间。有关示例，请参阅教程。[tutorials](http://wiki.ros.org/costmap_2d/Tutorials/Configuring Layered Costmaps)

坐标系和 tf 参数

~<name>/global_frame(string, default:"/map")

成本地图在其中运行的全局框架。

~/robot_base_frame

机器人基础链接的框架名称。

~/transform_tolerance

(double, default: 0.2)

指定可容忍的转换 (tf) 数据延迟（以秒为单位）。此参数用作在 tf 树中丢失链接的保护措施，同时仍允许用户在系统中存在的延迟量是舒适的。例如，转换过时 0.2 秒可能是可以容忍的，但转换过时 8 秒则不行。如果由 global_frame 和 robot_base_frame 参数指定的坐标系之间的 tf 转换早于 ros::Time::now() 的 transform_tolerance 秒，则导航堆栈将停止机器人。

速率参数

~<name>/update_frequency(double, default: 5.0)

要更新地图的频率（以赫兹为单位）。

~/publish_frequency

(double, default: 0.0)

地图发布显示信息的频率（以赫兹为单位）。

地图管理参数

~/rolling_window(bool, default: false)

是否使用代价地图的滚动窗口版本。如果 static_map 参数设置为 true，则此参数必须设置为 false。

~/always_send_full_costmap

(bool, default: false)

如果为真，完整的代价地图将在每次更新时发布到“~/costmap”。如果为 false，则只有已更改的成本地图部分发布在“~/costmap_updates”主题上。

以下参数可以被某些层覆盖，即静态地图层。

~<name>/width (int, default: 10)

地图的宽度（以米为单位）。

~/height

(int, default: 10)

地图的高度（以米为单位）。

~/resolution

(double, default: 0.05)

以米/单元为单位的地图分辨率。

~/origin_x

(double, default: 0.0)

地图在全局框架中的 x 原点，以米为单位。

~/origin_y

(double, default: 0.0)

地图在全球框架中的 y 原点，以米为单位。

8.1.5 所需的 tf 转换

(value of global_frame parameter) → (value of robot_base_frame parameter)

通常由负责里程计或定位的节点提供，例如 amcl。

8.1.6 C++ API

有关 costmap_2d::Costmap2DROS 类的 C++ 级 API 文档，请参阅以下页面：Costmap2DROS C++ API

8.2 图层规格

8.2.1 静态地图层

静态地图层代表代价地图的大部分不变部分，就像 SLAM 生成的那些一样。 static map layer

8.2.2 障碍贴图图层

障碍层跟踪传感器数据读取的障碍物。 ObstacleCostmapPlugin 在二维中标记和光线追踪障碍物，而 VoxelCostmapPlugin 在三维中这样做。 obstacle layer

8.2.3 膨胀层

膨胀层是一种优化，它在致命障碍物周围添加新值（即膨胀障碍物），以使成本图代表机器人的配置空间。inflation layer

8.2.4 其他层

其他层可以通过 pluginlib 在成本图中实现和使用。欢迎任何其他插件被列出并链接到下面。

Social Costmap Layer 社会代价地图层

Range Sensor Layer 范围传感器层