ROS Navigation-----costmap_2d简介

    这个包提供了一种2D代价地图的实现方案，该方案利用输入传感器数据，构建数据2D或者3D（依赖于是否使用基于voxel的实现）占用珊格，以及基于占用珊格和用户定义膨胀半径的2D代价地图的膨胀代价。 此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和配置传感器主题。

1 概述

注意: 在上图中，红色cell代表的是代价地图中的障碍，蓝色cell代表的是通过机器人内切圆半径计算的障碍物膨胀，红色多边形代表的是机器人footprint。 为了使机器人不碰到障碍物，机器人的footprint绝对不允许与红色cell相交，机器人的中心绝对不允许与蓝色cell相交。 

    costmap_2d包提供了一种可配置框架来维护机器人在占用珊格应该如何导航的信息。 代价地图使用来自传感器的数据和来自静态地图中的信息，通过costmap_2d::Costmap2DROS来存储和更新现实世界中障碍物信息。 costmap_2d::Costmap2DROS给用户提供了纯2D的接口，这意味着查询障碍只能在列上进行。例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。 这种设计对平面空间进行路径规划是有帮助的。

    从Hydro发布版本开始， 用来写数据到代价地图的底层方法已经完全可配置了。 每种功能放置一层中。 例如，静态地图是一层，障碍物是另一层。 缺省情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。

   该包提供的ROS化功能接口主要就是costmap_2d::Costmap2DROS，它使用costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改。costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。

   关于代价地图如何更新占用珊格的细节将在下面介绍，同时提供了页面链接供查看某一层具体如何工作。

2障碍物标记和清除

    代价地图自动订阅传感器发布的主题并基于数据进行相应自我更新。 对每个传感器来说，其可以用来执行mark（将障碍物信息插入到代价地图），也可以用来执行clear（从代价地图移除障碍物）或者二者都执行。marking操作就是索引到数组内修改cell的代价。然而对于clearing操作，每次观测报告都需要传感器源向外发射线，由射线穿过的珊格组成。 如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。

3 空间状态（Occupied, Free, and Unknown）

    虽然代价地图中每个cell可用255个不同值中任何一个值(see the inflation section)，可是下层数据结构仅需要3个值。 具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值。 如果列有一定量的占用(see mark_threshold parameter) 就被赋代价值 costmap_2d::LETHAL_OBSTACLE， 如果列有一定量的unknown cells (see unknown_threshold parameter) 就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE。

4 地图更新

   代价地图以参数update_frequency 指定的周期进行地图更新。每个周期传感器数据进来后，都要在代价地图底层占用结构上执行标记和清除障碍操作，并且这种结构会被投影到代价地图附上相应代价值。 这完成之后，对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作，即从每个占用cell向外传播代价值，直到用户定义的膨胀半径为止。

5 tf

   为了把来自传感器源的数据插入到代价地图，costmap_2d::Costmap2DROS要大量使用tf。 costmap_2d::Costmap2DROS 假定全局坐标系，基座坐标系和传感器源之间的转换有联系且是最新的。转换 transform_tolerance 参数指定这些转换之间的最大延迟。 如果 tf 树没有以期望速度被更新，那么导航功能包集将会让机器人停止。

6 膨胀

膨胀就是从占用cell向外传播代价值的过程，随着传播距离增加代价值会递减。 为实现这个过程，为代价值定义了5种特定表示符号：

• "Lethal" cost：意思是cell内有障碍物，因此如果机器人中心位于那个cell，很显然机器人会碰到障碍
• "Inscribed" cost：意思是cell到障碍物的距离小于机器人内切圆半径。 因此如果机器人中心在cell（或者代价>=内切代价），机器人与障碍物肯定有冲突

• "Possibly circumscribed" cost：类似于内切，但是使用机器人外接圆作为截止距离。因此，如果机器人中心位于cell上（或者代价>=外接代价），那么它与障碍物是否冲突依赖于机器人方位。使用术语“可能”意思是这不一定是一个真正障碍cell，但是有些用户喜欢放特定的代价值进去为特定目的。 例如，如果用户要表示机器人应该尝试避开建筑物某特定区域， 它们可以在代价地图上为那个与任何障碍毫无关系的区域（该区域本来就不是障碍区）插入代价值。注意，虽然上图示例中使用了代价值128，但是实际由于代价值受内切圆半径和外接圆半径的影响，真正实际值并不一定是128。

• "Freespace" cost： 假定为0，意思是没啥东西会阻碍机器人到那里
• "Unknown" cost：意思是cell上没有信息
• All other costs： 被赋值为"Freespace"和"Possibly circumscribed"之间的一个值，取决于到 "Lethal" cell的距离以及用户定义的衰减函数

7 地图类型

   有2种初始化costmap_2d::Costmap2DROS对象的方法。 第一种是把用户创建的静态地图提供给它 (see the map_server package for documentation on building a map)，代价地图被初始化来匹配静态地图宽，高以及静态地图提供的障碍物信息。这种方式通常与定位系统（如AMCL）结合使用，允许机器人在地图坐标系注册障碍物，并且当基座在环境中移动时可以根据传感器数据更新其代价地图。

    第二种初始化 costmap_2d::Costmap2DROS的方式是给出宽，高并且设置 参数 rolling_window 为true。 rolling_window 参数可以确保机器人在移动时候始终位于代价地图的中心位置，当机器人移动距离给定区域很远后丢弃原先区域的障碍信息。这种配置通常用于里程坐标系，这时机器人只care周围局部区域的障碍物信息。

8 组件API

8.1 Costmap2DROS

  costmap_2d::Costmap2DROS是对costmap_2d::Costmap2D的封装，将其功能以C++ ROS Wrapper导出，可以在初始化时指定的ROS命名空间使用。

如下是初始化costmap_2d::Costmap2DROS 的一个示例:

#include 
#include 

...

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

Note that the C++ API has changed as of Hydro.

8.1.1 ROS API

Note on Old Parameters

   为便于客制化，许多参数从Hydro版本开始改变了位置。

8.1.2 Subscribed Topics

~/footprint ( geometry_msgs/Polygon)

• 机器人footprint规范

8.1.3 Published Topics

~/grid ( nav_msgs/OccupancyGrid)

• The values in the costmap

~/grid_updates ( map_msgs/OccupancyGridUpdate)

• The value of the updated area of the costmap

~/voxel_grid ( costmap_2d/VoxelGrid)

• Optionally advertised when the underlying occupancy grid uses voxels and the user requests the voxel grid be published.

8.1.4 Parameters

 costmap_2d::Costmap2DROS 是高度可配置的，使用的ROS参数分为如下几组：coordinate frame and tf, rate, robot description and map management.

(1）Coordinate frame and tf parameters

~/global_frame (string, default: "/map")

• The global frame for the costmap to operate in.

~/robot_base_frame ( string, default: "base_link")

• The name of the frame for the base link of the robot.

~/transform_tolerance ( double, default: 0.2)

• Specifies the delay in transform (tf) data that is tolerable in seconds. This parameter serves as a safeguard to losing a link in the tf tree while still allowing an amount of latency the user is comfortable with to exist in the system. For example, a transform being 0.2 seconds out-of-date may be tolerable, but a transform being 8 seconds out of date is not. If the tf transform between the coordinate frames specified by the global_frame and robot_base_frame parameters is transform_tolerance seconds older than ros::Time::now(), then the navigation stack will stop the robot.

（2）Rate parameters

~/update_frequency (double, default: 5.0)

• The frequency in Hz for the map to be updated.

~/publish_frequency ( double, default: 0.0)

• The frequency in Hz for the map to be publish display information.

（3）Map management parameters

~/rolling_window (bool, default: false)

• Whether or not to use a rolling window version of the costmap. If the static_map parameter is set to true, this parameter must be set to false.

~/always_send_full_costmap ( bool, default: false)

• If true the full costmap is published to "~/grid" every update. If false only the part of the costmap that has changed is published on the "~/grid_updates" topic.

The following parameters can be overwritten by some layers, namely the static map layer.

• ~/width (int, default: 10)
  • The width of the map in meters.
  ~/height (int, default: 10)
  • The height of the map in meters.
  ~/resolution (double, default: 0.05)
  • The resolution of the map in meters/cell.
  ~/origin_x (double, default: 0.0)
  • The x origin of the map in the global frame in meters.
  ~/origin_y (double, default: 0.0)
  • The y origin of the map in the global frame in meters.

8.1.5 Required tf Transforms

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

• Usually provided by a node responsible for odometry or localization such as amcl.

8.1.6 C++ API

For C++ level API documentation on the costmap_2d::Costmap2DROS class, please see the following page: Costmap2DROS C++ API

8.2 Layer Specifications

8.2.1 Static Map Layer

The static map layer represents a largely unchanging portion of the costmap, like those generated by SLAM.

8.2.2 Obstacle Map Layer

The obstacle layer tracks the obstacles as read by the sensor data. The ObstacleCostmapPlugin marks and raytraces obstacles in two dimensions, while the VoxelCostmapPlugin does so in three dimensions.

8.2.3 Inflation Layer

The inflation layer is an optimization that adds new values around lethal obstacles (i.e. inflates the obstacles) in order to make the costmap represent the configuration space of the robot.

8.2.4 Other Layers

Other layers can be implemented and used in the costmap via pluginlib. Any additional plugins are welcomed to be listed and linked to below.

• Social Costmap Layer

• Range Sensor Layer