增加超声波传感器

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

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

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

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

总体而言，雷达发送的数据，发送到/scan这个topic,数据会被保存到layeredCostmap里，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.)

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

costmap2dros还建立一个一个线程Costmap2DROS::mapUpdateLoop，该线程负责不断循环得到当前的robot位置，然后让layercostmap知道当前的robot位置，并让每个pluginlayer计算一个bounds(updateBounds),以此bound作为localmap的大小，并取出每个plugin的数据(updateCosts)，将数据合并或者覆盖（具体策略取决于上面建立layer时的一个flag:combination_method）从而算出最终的costmap,这个最终的costmap就是layeredcostmap里面的一个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

->voidObstacleLayer::updateBounds

->getMarkingObservations

-> getObservations

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

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

ObstacleLayer 继承自CostmapLayer继承自costmap2d.

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