沉默杀手HomeArchivesTagsCategories
代价地图(二) 话题服务和部分源码
|路径规划代价地图|
Word count: 838|Reading time: 2 min

平常所说的costmap（代价地图）有下面三层，这三层组合成了master map（最终的costmap），供给路线规划模块使用。：

  • 静态层： 为了做全局规划，机器人需要一个超越其传感器的地图，以了解墙壁和其他静态障碍物的位置。 静态地图可以用SLAM算法生成，它表示了costmap中不会变化的大部分

  • 障碍层： 该层从诸如激光和RGB-D相机的高精度传感器收集数据，并将其放置在二维网格中，追踪障碍物。 ObstacleCostmapPlugin在二维层面标记和射线追踪障碍物，theVoxelCostmapPlugin是从三维追踪。

  • 膨胀层： 在每个致命障碍周围插入缓冲区，增加到costmap的值取决于离最近的障碍的距离(膨胀半径)。机器人肯定会发生碰撞的地点有致命的代价，而且周围的区域有一些小的非致命的成本。这些值确保机器人不会撞上致命的障碍物，也不愿太靠近

文件夹costmap_2d\plugins提供了这三层和voxel层的源文件，costmap_2d包中的costmap_plugins.xml定义了这几层和VoxelLayer。

我自己定义的障碍物也可以是一层，假如我不想让机器人通过一个freespace就可以自己插入个障碍物，主要的接口是costmap_2d::Costmap2DROS，在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap，各个层可以被独立的编译。写法模仿obstacle_layer.cpp，在CMakeLists里添加后，一起编译成liblayers.so

rotate_recovery, move_base, move_slow_and_clear, global_planner, clear_costmap_recovery(这个库又在move_base中调用), dwa_local_planner, base_local_planner都使用了libcostmap_2d.so这个库

src文件夹中的costmap_2d_markers.cpp, costmap_2d_cloud.cpp, costmap_2d_node.cpp各自编译成一个可执行文件,其余文件联合编译成库文件libcostmap_2d.so

costmap相关的话题和服务

省略每一层和两个代价地图的parameter_descriptions和parameter_updates。以parameter_descriptions 和 parameter_updates结尾的话题用于dynamic_reconfigure机制，不必关注。echo话题会只获得一次消息，几乎不占用带宽，用rostopic bw没有结果。

1
2
3
4
5
6
7
8
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint

# 局部代价地图的区别仅仅是没有静态层
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint

/move_base/global_costmap/costmap 的类型是 nav_msgs/OccupancyGrid，所以不能直接订阅，需要转为代价地图.

参数~<name>/always_send_full_costmap,   (bool, default: false). 如果为true，那么每次更新，完整的costmap都会发布到话题~<name>/costmap. 如果为false， 只有部分 costmap 更新时，会发布到话题 ~<name>/costmap_updates。 设置always_send_full_costmap为false后，带宽大幅减少

话题~<name>/costmap_updates， 类型map_msgs/OccupancyGridUpdate，the value of the updated area of the costmap

除了两个代价地图和每一层的set_parameters服务之外，服务就只有/move_base/clear_costmaps

服务set_parameters是由 Dynamic Parameters server创建，我在导航框架的源码里没找到这个关键词，怀疑它的发布在ROS源码里。

部分源码

move_base里初始化代价地图是使用的Costmap2DROS， move_base里没有直接使用过Costmap2D类的对象

1
2
3
4
5
6
7
8
9
10
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
// 只有两行，类成员变量赋值： stop_updates_ = true;  initialized_ = false;
planner_costmap_ros_->pause();

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
// 省略... 初始化global_planner   local_planner
// 很简单的代码，对各层地图插件调用activate函数，激活各层地图
planner_costmap_ros_->start();
controller_costmap_ros_->start();