在数据成员中,有两个重要的变量:Costmap2D costmap_;
和 std::vector<boost::shared_ptr<Layer> > plugins_;
。
这个类相对比较简单,首先来看构造函数:
LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) :
costmap_(), global_frame_(global_frame), rolling_window_(rolling_window), initialized_(false), size_locked_(false)
{
if (track_unknown)
costmap_.setDefaultValue(255);
else
costmap_.setDefaultValue(0);
}
调用costmap_
的setDefaultValue
方法,实际上设定了类costmap_2d
的一个成员变量default_value_
这个值在class costmap_2d
中是这样使用的:memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
实际存储地图的变量就是class costmap_2d
的 costmap_
数据成员。
析构函数中,所有的操作就是弹出plugin: plugins_.pop_back();
。
函数LayeredCostmap::resizeMap
就是给class costmap_2d
的 costmap_
成员的大小重新做分配。然后根据plugin对每一层的地图调用其父类Costmap2D
成员的initial
方法,实际效果就是将plugin所指向的每一层地图的大小都设置为和LayeredCostmap::costmap_
数据成员一样的空间大小。
{
size_locked_ = size_locked;
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->matchSize();
}
}
函数 LayeredCostmap::updateMap
完成对每一层地图的更新,更新过程分为两步updateBounds
和updateCosts
:
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
if (rolling_window_)
{
double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
costmap_.updateOrigin(new_origin_x, new_origin_y);
}
if (plugins_.size() == 0)
return;
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -