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发送出去。