ROS与navigation教程-costmap_2d

ROS与navigation教程-costmap_2d

说明：

• 介绍 costmap_2d 的概念和相关知识。

代码库

• 参考链接：https://github.com/ros-planning/navigation

概要

• costmap_2d包提供了一种2D代价地图的实现方案，该方案从实际环境中获取传感器数据，构建数据的2D或3D占用栅格（取决于是否使用基于体素的实现），以及基于占用栅格和用户定义膨胀半径的2D代价地图的膨胀代价。

• 该包也支持基于map_server初始化代价地图，基于滚动窗口的代价地图，以及基于参数化订阅和配置传感器主题。

• 注意：在上图中，红色单元表示代价地图中的障碍物，蓝色单元表示由机器人内切半径来计算膨胀的障碍物，红色多边形表示机器人的垂直投影（footprint）。 为了避免机器人与障碍碰撞，机器人的垂直投影（footprint）不能与红色单元相交，机器人的中心不能穿过蓝色单元。

• costmap_2d包提供了一种可配置框架来维护机器人在占用栅格应该如何导航的信息。代价地图使用传感器数据和来自静态地图的信息，通过costmap_2d :: Costmap2DROS对象来存储和更新实际环境中的障碍物信息。costmap_2d :: Costmap2DROS对象为其用户提供纯2D接口，这意味着关于障碍的查询只能在列中进行。 例如，在XY平面中具有不同Z位置的相同位置的表和鞋将导致costmap_2d :: Costmap2DROS对象的代价地图中具有相同成本值的相应单元格。这种设计对平面空间进行路径规划是有帮助的。

• 从Hydro发布版本开始， 用来写数据到代价地图的底层方法已经完全可配置了。每种功能放置一层中。 例如，静态地图是一层，障碍物是另一层。默认情况下，障碍层维护3D信息（参阅voxel_grid）。维护3D障碍数据可以使图层更智能地处理标记和清除。

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

• 有关代价地图如何更新占用栅格的详细信息将会在下面提到，同时提供了页面链接供查看某一层具体如何工作。

Marking and Clearing

• 成本图自动预订ROS上的传感器话题，并相应进行更新。每个传感器可以执行标记操作（将障碍物信息插入到成本图中），清除操作（从成本图中删除障碍物信息）或两者都同时执行。标记操作就是索引到数组内修改单元格的代价。然而对于清除操作，每次观测报告都需要传感器源向外发射线。如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。

Occupied, Free, and Unknown Space

• 虽然代价图中的每个单元格可以具有255个不同的代价值之一（见“inflation”部分），但下层数据结构仅需要3个值。在该下层结构中的每个单元有三种状态：自由的，占用的或未知的。 每个状态在投影到代价图中后都会有其特定的代价值。每个状态在投影到代价图中后都会有其特定的代价值。 具有一定数量的占用单元格的列（参阅mark_threshold参数）会被分配了costmap_2d :: LETHAL_OBSTACLE 代价值;具有一定数量的未知单元格的列（参见unknown_threshold参数）被分配了costmap_2d :: NO_INFORMATION 代价值，剩余的其他列分配了costmap_2d :: FREE_SPACE 代价值。

Map Updates

• 代价地图以参数update_frequency 指定的周期进行地图更新。每个周期获取传感器数据后，代价地图底层占用结构都会执行标记和清除操作，并且将该结构投影到代价图且附上相应代价值。然后，每个代价值为costmap_2d::LETHAL_OBSTACLE的单元格都会执行障碍物膨胀操作，即从每个占用单元格向外传播代价值，直到用户定义的膨胀半径为止。

tf

• 为了把来自传感器源的数据插入到代价地图，costmap_2d::Costmap2DROS要大量使用tf。 具体来说，它假定由global_frame参数，robot_base_frame参数和传感器源指定的坐标系之间的所有变换都是有联系的且是最新的。transfor_tolerance 参数设置了这些变换之间的最大延迟量（amount of latencylatency）。如果 tf 树没有以期望速度来更新，那么导航功能包将会让机器人停止。

Inflation

• 膨胀代价值随着机器人离障碍物距离增加而减少。

• 为代价地图的代价值定义5个与机器人有关的标记：

  • 致命的（"Lethal" cost）：说明该单元格中存在一个实际的障碍。若机器人的中心在该单元格中，机器人必然会跟障碍物相撞。

  • 内切（"Inscribed" cost）：说明该单元格离障碍物的距离小于机器人内切圆半径。若机器人的中心位于等于或高于"Inscribed" cost的单元格，机器人必然会跟障碍物相撞。

  • 可能外切（"Possibly circumscribed" cost ）：说明一个单元格离障碍物的距离小于机器人外切圆半径，但是大于内切圆半径。若机器人的中心位于等于或高于"Possibly circumscribed" cost 的单元格，机器不一定会跟障碍物相撞，其取决于机器人的方位。

  • 自由空间（"Freespace" ）：没有障碍的空间。

  • 未知（"Unknown"） ： 未知空间。

  • All other costs are assigned a value between "Freespace" and "Possibly circumscribed" depending on their distance from a "Lethal" cell and the decay function provided by the user.

• The rationale behind these definitions is that we leave it up to planner implementations to care or not about the exact footprint, yet give them enough information that they can incur the cost of tracing out the footprint only in situations where the orientation actually matters.

Map Types

• 初始化costmap_2d :: Costmap2DROS对象的方法主要有两种。

• 第一种是使用用户生成的静态地图进行初始化（有关构建地图的文档，请参阅map_server包）。在这种情况下，代价地图被初始化来匹配与静态地图提供的宽度，高度和障碍物信息。这种方式通常与定位系统（如AMCL）结合使用，允许机器人在地图坐标系注册障碍物，并且当基座在环境中移动时可以根据传感器数据更新其代价地图。

• 第二种是给出一个宽度和高度，并将rolling_window参数设置为true。当机器人在移动时，rolling_window参数将机器人保持在成本图的中心。然而当机器人在给定区域移动得太远时，障碍物信息将会被从地图中丢弃。这种方式的配置最常用于里程计坐标系中，机器人只关注局部区域内的障碍物。

Component API

( 1 ) Costmap2DROS

• costmap_2d::Costmap2DROS是对costmap_2d::Costmap2D的封装，将其功能以C++ ROS Wrapper导出，可以在初始化时指定的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命名空间内运行。在这种情况下，所有对下面的名称的引用都应该被替换成costmap。

• 更常见的情况是通过启动move_base节点来运行完整的导航功能包。 这将创建2个costmaps，每个都有自己的命名空间：local_costmap和global_costmap。 这就可能需要为每个代价地图设置参数。

( 1.1 ) ROS API

The C++ API has changed as of Hydro.

( 1.2 )

~<name>/footprint ([geometry_msgs/Polygon][7]) 

• 机器人脚印的规格。This replaces the previous parameter specification of the footprint.

( 1.3 ) Published Topics

~<name>/grid ([nav_msgs/OccupancyGrid][8]) 

• 代价地图的值。

• The values in the costmap .

~<name>/grid_updates ([map_msgs/OccupancyGridUpdate][9]) 

• 代价图的更新区域的值。

~<name>/voxel_grid ([costmap_2d/VoxelGrid][10]) 

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

( 1.4 ) Parameters

• Hydro和更高版本使用所有costmap_2d层的插件。如果您没有提供插件参数，那么初始化代码将假定您的配置为Hydro，并将使用默认命名空间加载默认的插件集。您的参数将自动移动到新的命名空间。默认的命名空间是static_layer，barriers_layer和inflation_layer。同时要注意一些教程（和书籍）仍然是用Hydro前的参数。所以请务必提供一个插件参数。

Plugins

~<name>/plugins (sequenc

e, default: pre-Hydro behavior)

• 插件说明的顺序，每层一个。每个说明都是具有名称和类型字段的字典。该名称用来定义插件的参数命名空间。

• 有关示例，请参阅tutorials。

Coordinate frame and tf parameters

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

• The global frame for the costmap to operate in.

~<name>/robot_base_frame (string, default: "base_link") 

• 机器人base_link的坐标系名称。

~<name>/transform_tolerance (double, default: 0.2) 

• 指定在transform（tf）数据中可接受的延迟，单位为 seconds 。

• 此参数可用于在tf树中丢失链接的情况，同时仍允许用户在系统中存在一定的延迟。

• 例如，0.2秒超时的变换可能是可以接受的，但变化为8秒过期的转换不是。如果一个tf变换超过了tolerance限定的时间，机器人将被停止。

Rate parameters

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

• 地图更新的频率，单位为 Hz 。

~<name>/publish_frequency (double, default: 0.0)

• 地图发布到显示信息的频率。

Map management parameters

~<name>/rolling_window (bool, default: false) 

• 是否使用滚动窗口模式的代价地图。

• 如果static_map参数设置为true，则此参数必须设置为false。

~<name>/always_send_full_costmap (bool, default: false) 

• 假若为true，则每次更新都会将完整的代价地图发布到 "~/grid"。但若为false，则仅在 "~/grid" 话题上发布已更改的代价地图的部分。

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

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

• 地图宽度，单位为 meters 。

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

• 地图高度，单位 meters 。

~<name>/resolution (double, default: 0.05) 

• 地图分辨率，单位 meters 。

~<name>/origin_x (double, default: 0.0) 

• 全局地图中的x原点，单位 meters 。

~<name>/origin_y (double, default: 0.0) 

• 全局地图中的y原点，单位 meters 。

( 1.5 )Required tf Transforms

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

• 通常都是由负责里程计或者定位的节点，比如amcl节点，来提供这个tf变换。

( 1.6 )C++ API

• 有关costmap_2d :: Costmap2DROS类的C ++levelAPI文档，请参阅：Costmap2DROS C ++ API。

( 2 ) Layer Specifications

( 2.1 ) Static Map Layer

• 静态地图层代表代价地图的一个很大的不变部分，就像用SLAM生成的一样。

( 2.2 ) Obstacle Map Layer

• 障碍层读取传感器数据来追踪障碍物。 ObstacleCostmapPlugin标记和光线追踪2D的障碍，而VoxelCostmapPlugin也是用同样的方式追踪3D的障碍。

( 2.3 ) Inflation Layer

• 膨胀层是对致命障碍（lethal obstacles）周围边界的扩展（即是障碍物膨胀），以便代价地图更清晰显示机器人的可利用空间。

( 2.4 ) Other Layers

• 其他层可以通过pluginlib在costmap中实现和使用。

  • Social Costmap Layer

  • Range Sensor Layer

参考资料

• http://wiki.ros.org/costmap_2d/

• http://blog.csdn.net/sonictl/article/details/51518492

• http://blog.csdn.net/x_r_su/article/details/53408528