给local map增加超声波传感器数据的各个步骤

1.首先要定义urdf。定义好要使用的sensor的角度，位置，和frame名字。

2.然后将sensor数据在kobuki底座中读取出来，发送到一个topic上。

3.给costmap2d中增加一个layer监听上述topic.

一些修改配置文件的资料可见：

http://wiki.ros.org/navigation/Tutorials/RobotSetup

costmap背景介绍：

move_base package包含了planner，用于根据指定的目的地规划路径。他含有两个costmap,一个global的，一个local的。

planner_costmap_ros_ =new costmap_2d::Costmap2DROS("global_costmap", tf_);

controller_costmap_ros_= new costmap_2d::Costmap2DROS("local_costmap", tf_);

就超声波而言，只需要将数据发送到move_base的一localcostmap就可以了。

costmap2dros

如何给localcostmap增加一个layer:

雷达收到的数据，发送到/scan这个topic,该数据会被保存到layeredCostmap的layer里面，layerdCostmap里面有若干layer,每个layer是一个plugin.比如ObstacleLayer就是一个plugin.如果需创建一个新layer，需要修改配置文件/turtlebot_navigation/param/costmap_common_params.yaml，在其中定义一个layer.(注意所添加的layer的参数，中比如 min_obstacle_height:0.25代表了障碍物的最低高度，如果你的sensor的高度比这个还要低，那么这个sensor发出的数据没有办法保存到此layer里面。我当初就是给rplidar定义layer时，给rplidar定义的urdf文件中指定了rplidar的高度是0.15,且rplidar这个雷达只水平探测，那么他他呢到的点的高度肯定也是0.15了，但是其layer定义所支持的障碍物最低高度0.25，导致我的雷达数据始终无法发送到costmap里面，结果就是我用rviz看图时，永远看不到localmap,并且rostopic监听costmap以及costmap_update得到的都是map数据为0.)要指定该layer监听的observationtopic, 以及topic的消息类型。还有resolution,地图尺寸(voxel)，还有障碍物的最大，最小高度等等。注意zresolution和zvoxel，需要确保z轴坐标/zrelution得到的数值小于zvoxel,否者该坐标被认为不再地图内部。zvoxel可以看作是地图z轴所能容纳的格子数目。

以还需要修改/turtlebot_navigation/param/local_costmap_params.yaml以指定新增的layer的名字和类型。

costmap地图数据是如何合成的，以及如何发送给rviz的：

costmap2dros会建立一个线程Costmap2DROS::mapUpdateLoop，该线程不断循环获取当前的robot位置，并通知给layercostmap，并调用每个pluginlayer的updateBounds获取他们的bounds,以此bound作为localmap的大小，并取出每个plugin的数据(updateCosts)，将多个layer的数据合并或者覆盖在一起得到一个最终的map数据（具体策略取决于上面建立layer时的一个flag:combination_method）,这个最终的costmap就是layeredcostmap里的costmap，是一个char数组。costmap2dros会用一个costmap2dpublisher将此数据发送出去，发送给topic:/move_base/local_costmap/costmap, 以及topic:/move_base/local_costmap/costmap_update, 前者是整个map,后者是局部update.当只有打开了rviz时，才会发送这个数据到此topic上。否则如果没有人监听，就不会发送。

我们可以用rostopicinfo以及rostopicecho来查看该topic的发送，使用者，以及数据更新。

Costmap2dros是一个wrapper, 他包含了layeredcostmap,供movebase使用。

costmap2dros建立的线程的调用栈：

Costmap2DROS::reconfigureCB()创建一thread:

map_update_thread_ = newboost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop,this, map_update_frequency));

->void Costmap2DROS::mapUpdateLoop(doublefrequency) //thread loop 函数

->void Costmap2DROS::updateMap()

->void LayeredCostmap::updateMap

->voidVoxelLayer::updateBounds

->getMarkingObservations

-> getObservations

获取observation中的地图数据，并找到每个点的位置，并将数据更新到地图中：

costmap_[index] = LETHAL_OBSTACLE;//这里的costmap_是一个数组，是costmap2d构造函数中调用initmaps创建的。

ObstacleLayer 继承自CostmapLayer继承自costmap2d.

至此，callback将数据更新到layer的costmap_缓冲中。