CostmapLayer类分析

CostmapLayer代价地图层

头文件：

#include <ros/ros.h>
#include <costmap_2d/layer.h> //地图层
#include <costmap_2d/layered_costmap.h> //层的代价地图

数据类型：

protected：
bool has_extra_bounds_; //边界值更新是否成功
private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; //增加最大和最小的边界值

类关系：

class CostmapLayer继承 public Layer、public Costmap2D

函数：

CostmapLayer() : has_extra_bounds_(false),
					extra_min_x_(1e6), extra_max_x_(-1e6),
					extra_min_y_(1e6), extra_max_y_(-1e6) {}
		作用：构造函数，集成的数据都是默认值

bool isDiscretized()
{
return true;
}
	作用：如果是离散化的

virtual void matchSize();
		作用：匹配地图，该插件在不同的地图有不同的匹配方式
void CostmapLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}

virtual void clearArea(int start_x, int start_y, int end_x, int end_y);
		作用：保留该区域，其他区域进行设置成为 NO_INFORMATION
void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y)
{
unsigned char* grid = getCharMap();
for(int x=0; x<(int)getSizeInCellsX(); x++){
bool xrange = x>start_x && x<end_x;

for(int y=0; y<(int)getSizeInCellsY(); y++){
if(xrange && y>start_y && y<end_y)
continue;//跳过划定的范围的区域
int index = getIndex(x,y);
if(grid[index]!=NO_INFORMATION){
grid[index] = NO_INFORMATION;
}
}
}
}
	分析：找到该区域，其他区域进行设置成为NO_INFORMATION

void addExtraBounds(double mx0, double my0, double mx1, double my1);
		作用： 如果外部源更改了costmap中的值， 则应使用更改的区域调用此方法，以确保costmap也包括此区域。
void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
{
extra_min_x_ = std::min(mx0, extra_min_x_);
extra_max_x_ = std::max(mx1, extra_max_x_);
extra_min_y_ = std::min(my0, extra_min_y_);
extra_max_y_ = std::max(my1, extra_max_y_);
has_extra_bounds_ = true;//表示已经更改地图的边界值，完成地图的扩展
}
		分析：更改地图的边界值

protected:

void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
	作用：使用此层的值更新指定边界框内的主网格，意味着该层中的每个值都写入主网格。
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();

for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
master[it] = costmap_[it];
it++;
}
}
}
		分析：全部进行覆盖，层中所有的值对主层进行覆盖

void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
		作用：覆盖意味着此层中的每个有效值都写入主网格（不复制未知值）对未知区域进行赋值
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)//判断是否可以进行更改地图代价值
return;
unsigned char* master = master_grid.getCharMap();//主代价图
unsigned int span = master_grid.getSizeInCellsX();

for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;//每一行的起点
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] != NO_INFORMATION)//如果代价地图是未知
master[it] = costmap_[it];//就把这个值写进这个地图，直接代替写入
it++;
}
}
}
		分析：就是一个地图对主代价图进行覆盖，未知区域就不往主代价图中写了

void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
	作用：将新值设置为主网格值和此层值的最大值，如果主值是未知区域值，它将被覆盖。如果层的值是未知的区域值，则主值不会更改。一个层对主层代价值进行赋值

void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)//根据enabled来判断是否需要更新地图
return;

unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();

for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}

unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
master_array[it] = costmap_[it];
it++;
}
}
}
		分析：分成两个层的地图代价值，对主图进行更新代价值，更新选择最大的值，255未知是最大

void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
		作用：将新值设置为主网格值和此层值的总和，如果主值是未知的区域值，它将被覆盖。如果层的值是未知的区域值，则主值不会更改。如果总和值大于膨胀代价值，则主值设置为（膨胀代价值-1）。
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)//判断是否进行操作
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();

for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}//去除层中的未知值255

unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION)//如果主值是255未知，直接赋值设置
master_array[it] = costmap_[it];
else//如果主值不是未知，主值和层值相加
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)//253-1，膨胀值
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
it++;
}
}
}
分析：一个层对主层进行操作，其中代价值进行叠加

void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y);
	作用：更新参数中指定的边界框以包括位置（x，y）
	//确定该坐标是否在这个范围中，如果不在更新边界值
void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
{
*min_x = std::min(x, *min_x);
*min_y = std::min(y, *min_y);
*max_x = std::max(x, *max_x);
*max_y = std::max(y, *max_y);
}
		分析：更新边界值进行将这个坐标加入地图中

void useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y);
		作用：更新指定的边界值，以包含addExtraBounds调用中的边界框，就是说如果addExtraBounds没有调用成功，或者是没有调用，就使用这个方法进行设置边界值
如果成功调用addExtraBounds，这个方法就不会被调用
void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!has_extra_bounds_)
return;

*min_x = std::min(extra_min_x_, *min_x);
*min_y = std::min(extra_min_y_, *min_y);
*max_x = std::max(extra_max_x_, *max_x);
*max_y = std::max(extra_max_y_, *max_y);
extra_min_x_ = 1e6;
extra_min_y_ = 1e6;
extra_max_x_ = -1e6;
extra_max_y_ = -1e6;
has_extra_bounds_ = false;
}

代码来源：Turtlebot3-navigation