movebase导航和地图数据的使用 - 10 static layer数据的生成和使用

Static layer的数据来源方式两种：

1. staticLayer可以从mapserver的maptopic直接接受数据。StaticLayer::incomingMap是callback.这种情况是直接使用已有的map数据。

2. 还有一种得到map数据的方法是从gmapping拿到实时发现的地图数据：

navigation/map_server/src/main.cpp:174: map_pub =n.advertise<nav_msgs::OccupancyGrid>("map",1, true);

slam_gmapping/gmapping/src/slam_gmapping.cpp:251: sst_ =node_.advertise<nav_msgs::OccupancyGrid>("map",1, true);

slam_gmapping/gmapping/src/slam_gmapping.cpp:266: sst_ =node_.advertise<nav_msgs::OccupancyGrid>("map", 1,true);

其中第2种方法中，slam_gmapping要监听scan数据，处理后，将地图发出发给maptopic：

SlamGMapping::laserCallback(constsensor_msgs::LaserScan::ConstPtr& scan)

→1. SlamGMapping::addScan(const sensor_msgs::LaserScan&scan, GMapping::OrientedPoint& gmap_pose)

将scan数据封装到GMapping::RangeReading里然后，调用boolGridSlamProcessor::processScan(const RangeReading & reading, intadaptParticles)处理这个scan.scan的数据会被保存到trajectorytree的节点里。

这里面设计到gridfast slam 算法，涉及到particle,Trajectory tree(由一些node构成)，

2. SlamGMapping::updateMap(const sensor_msgs::LaserScan&scan)

使用上面保存到trajectorytree里面的数据。这个函数最终会发送地图。

首先先构建一个GMapping::ScanMatcherMap结构，这个结构用来包含地图的cell。如何填充它？是靠GMapping::ScanMatcher::registerScan来填充的。

具体填充过程貌似是要遍历trajectorytree中的节点，从节点取出range数据。依靠之前设置的角度，beam数量。来计算各个点的位置填充每个cell.Trajectory tree中每个节点保存了一些激光数据。这些激光数据在之前的处理中，保存到了trajectorytree的节点里面的。

最后发送地图。

初步怀疑trajectorytree中记录的是轨迹数据，一个轨迹中，有多个位置，每个位置关联了一组从那个位置发现的laserrange数据。是发散的。而且多个位置的发散的激光range数据覆盖的内容区域肯定有重复。遍历所有位置并依次处理他们发现的区域就可以构建地图数据。

在staticlayer中，onInitialize()函数会等待收到map数据后才会继续执行。

Static layer收到map数据后，久存处在自己的costmap_里面了。

staticlayer数据的使用：

staticlayer作为layeredCostMap的一个plugin,其包含的数据迟早要被layeredCostmap取出来和其他layer数据合并。数据合并后，会被发送出去，发给costmap这个topic.而rviz就会接受到该数据。

合并的方法是对每个layer调用其updateCosts.staticLayer和voxelLayer（继承子obstaclelayer）不同，他的坐标系和layeredCostmap的坐标系可能不同，甚至基于不同的frame.不同的坐标系，可能仅仅涉及到平移等等，而基于不同的frame可能会还必须要考虑到frame之间已有的transform.比如旋转，缩放什么（可能会有缩放）的。那么看下面代码看看staticlayer是如何将自己的数据和别的layer合并的：

void StaticLayer::updateCosts(costmap_2d::Costmap2D&master_grid, int min_i, int min_j, int max_i, int max_j)

{

//master_grid是layeredCostMap中的内容，他可能已经包含了其他layer的合并结果。staticlayer也要把述据合并到这里面。min_i, min_j, max_i,,,代表master_grid中的最小最大坐标索引，也就是2维数组的索引。

if (!map_received_)

return;

if (!layered_costmap_->isRolling())

{

// if not rolling, the layered costmap (master_grid) has samecoordinates as this layer

if (!use_maximum_)

updateWithTrueOverwrite(master_grid, min_i, min_j, max_i,max_j);

else

updateWithMax(master_grid, min_i, min_j, max_i, max_j);

}

else

{

// If rolling window, the master_grid is unlikely to have samecoordinates as this layer

unsigned int mx, my;

double wx, wy;

// Might even be in adifferent frame ，甚至frame都不同

//因为我们有很多layer要和并，那么以globalframe作为基准比较合适，而staticlayer是基于mapframe的，所以我们肯定要用到globalfram到mapframe的transform.

tf::StampedTransform transform;

try

{

tf_->lookupTransform(map_frame_,global_frame_, ros::Time(0), transform);

}

catch (tf::TransformException ex)

{

ROS_ERROR("%s", ex.what());

return;

}

// Copy map data given proper transformations

for (unsigned int i = min_i; i < max_i; ++i)

{

for (unsigned int j = min_j; j < max_j; ++j)

{

//i, j 是mastergrid中的索引，是整数，并且这个索引不是ros坐标系中的坐标，ros坐标系中的坐标都是double类型，所以他们并不基于任何frame.只是一个2维数组的索引，需要把它转换成一个world的double类型的坐标。此外，更重要的是还需要注意这些2维数组索引对应的格子是有分辨率resolution的，所以需要调用mapToWorld().将格子索引坐标转换成world的基于米的真正坐标。得到的结果是基于global frame的world坐标系的坐标，转换如下：因为master_grida是layeredCostMap的，所以这里需要调用layeredCostMap的mapToWorld函数。

// Convert master_gridcoordinates (i,j) into global_frame_(wx,wy) coordinates

layered_costmap_->getCostmap()->mapToWorld(i, j, wx,wy);

// Transform fromglobal_frame_ to map_frame_

//因为staticlayer中的地图数据使用的是基于mapframe的（为什么这么说呢，因为StaticLayer::incomingMap这个函数收到的地图数据时，staiclayer的mapframe被赋值成这个地图数据携带的frameid）。所以要把基于globalframe的坐标转换成基于mapframe的坐标，才能对staticlayer中的数据有意义。所以要用到transform.

//global frame, 坐标=>map frame, 坐标

tf::Point p(wx, wy, 0);

p = transform(p);

// Set master_grid with cell from map

//再把基于mapframe的double的坐标转换成int的地图坐标，也就是转换成2维数组的索引坐标。这个转换需要处理resolution得到具体的格子数目。

那么，可以认为上述worldTomap和map2world操作更重要的意义是针对地图设置的分辨率，在double和int类型之间完成互相转换。而不是像transform那样进行坐标系的变幻。

if (worldToMap(p.x(),p.y(), mx, my))

{

//下面将数据取出，更新到对应位置的layerdCostMap里面

if (!use_maximum_)

master_grid.setCost(i, j, getCost(mx, my));

else

master_grid.setCost(i, j, std::max(getCost(mx, my),master_grid.getCost(i, j)));

}

}

}

}

}

总结scan数据到地图数据各个layer的流程：

1. 建图时，slam_gmapping,监听/scantopic将数据处理保存后，发送给maptopic. 使用fastslam算法建图。

2.导航时，如果不建图，没有启动gmapping的话，需要给mapserver配置地图。那么mapserver启动后，加载地图文件后，会向maptopic发送地图。

3. movebase的costmap2d模块的staticlayer会监听map和map_update topic, 得到地图，和地图更新。这个staticlayer位于globalcostmap内部。

4. movebase有建立2个costmap2dros,他们都有自己的layeredcostmap,costmap2dros有一个线程，循环调用layeredCostMap::updateMap,它把各个layer的地图数据合并，然后向costmaptopic发送出去。