Costmap2D类解析

Costmap2D

Costmap2D是turtlebot3中的基础，详细解析该类可以帮助其他类的解析以及使用。

**数据结构：**
unsigned int size_x_;表示x方向最大距离（x方向像素点个数）
unsigned int size_y_;表示y方向最大距离（y方向像素点个数）
double resolution_;分辨率大小，像素点之间的距离
double origin_x_;地图的下标计数原点
double origin_y_;
unsigned char* costmap_;  //地图数据
unsigned char default_value_;//默认值代价值
mutex_t* access_; //地图计算时，进行线程锁

//数据结构，用于进行位置记录（定位）
struct MapLocation
{
unsigned int x;
unsigned int y;
};

函数：

	Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
				double origin_x, double origin_y, unsigned char default_value = 0);
	构造函数：（cells_size_x， cells_size_y：地图边长
			   （origin_x， origin_y）：地图起点（或者原点）
				 resolution：分辨率    default_value：设定代价值

Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) :
size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
access_ = new mutex_t();

// create the costmap
initMaps(size_x_, size_y_);
resetMaps();
}

Costmap2D(const Costmap2D& map);
	构造函数：引用赋值拷贝函数（不可改变的赋值）

Costmap2D::Costmap2D(const Costmap2D& map) :
costmap_(NULL)
{
access_ = new mutex_t();
*this = map;
}

Costmap2D& operator=(const Costmap2D& map);
	构造函数：复制拷贝，对等号重载

Costmap2D& Costmap2D::operator=(const Costmap2D& map)
{
// check for self assignement
if (this == &map)
return *this;

// clean up old data
deleteMaps();

size_x_ = map.size_x_;
size_y_ = map.size_y_;
resolution_ = map.resolution_;
origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_;

// initialize our various maps
initMaps(size_x_, size_y_);

// copy the cost map
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));

return *this;
}

Costmap2D();
	构造函数：默认构造函数（数据不赋值,选择默认值）

Costmap2D::Costmap2D() :
size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
{
access_ = new mutex_t();
}

bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double 										win_origin_y, double win_size_x,double win_size_y);
	作用：将代价地图作为一个地图窗口副本放入地图

bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,double win_size_y)
{
// check for self windowing
if (this == &map)
{
 ROS_ERROR("Cannot convert this costmap into a window of itself");
return false;
}

// clean up old data
deleteMaps();

// compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
{
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return false;
}

size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;

// initialize our various maps and reset markers for inflation
initMaps(size_x_, size_y_);

// copy the window of the static map and the costmap that we're taking
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
return true;
}

	virtual ~Costmap2D();
		作用：析构函数
Costmap2D::~Costmap2D()
{
deleteMaps();
delete access_;
}

unsigned char getCost(unsigned int mx, unsigned int my) const;
	作用：获取一个单元（mx, my）,返回这个单元的代价值

unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
{
return costmap_[getIndex(mx, my)];
}

void setCost(unsigned int mx, unsigned int my, unsigned char cost);
	作用：设定一个单元（mx, my）的代价值

void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
costmap_[getIndex(mx, my)] = cost;
}

void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
	作用：从地图坐标（mx, my）向世界坐标(wx, wy)转换,一定会转换成功，地图坐标系小于世界坐标系

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}

bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
	作用：从世界坐标（wx, wy）向地图坐标(mx, my)转换,不一定会转换成功，地图坐标系小于世界坐标系，如果转换成功（合法界限），则为True；否则为false

bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
if (wx < origin_x_ || wy < origin_y_)
return false;

mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);

if (mx < size_x_ && my < size_y_)
return true;

return false;
}

void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
	作用：从世界坐标（wx, wy）向地图坐标(mx, my)转换,地图没有边界

void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
}

void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
	作用：从世界坐标（wx, wy）向地图坐标(mx, my)转换,地图有边界，从世界坐标转换为地图坐标，将结果约束为合法边界，返回的地图坐标保证位于地图内。

void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
{
// Here we avoid doing any math to wx,wy before comparing them to
// the bounds, so their values can go out to the max and min values
// of double floating point.
if (wx < origin_x_)
{
mx = 0;
}
else if (wx >= resolution_ * size_x_ + origin_x_)
{
mx = size_x_ - 1;
}
else
{
mx = (int)((wx - origin_x_) / resolution_);
}

if (wy < origin_y_)
{
my = 0;
}
else if (wy >= resolution_ * size_y_ + origin_y_)
{
my = size_y_ - 1;
}
else
{
my = (int)((wy - origin_y_) / resolution_);
}
}

inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
	return my * size_x_ + mx;
}
	作用：获取地图栅格地图下标值

inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
my = index / size_x_;
mx = index - (my * size_x_);
}

	作用：根据下表进行地图参数推算，下标数变成坐标（mx, my）

unsigned char* getCharMap() const;
	作用：获取到这个地图中存储代价地图的指针，通过指针找到地图信息

	unsigned char* Costmap2D::getCharMap() const
{
return costmap_;
}

unsigned int getSizeInCellsX() const;
unsigned int getSizeInCellsY() const;
	作用：获得代价地图的x、y边长（以栅格为单位）

unsigned int Costmap2D::getSizeInCellsX() const
{
return size_x_;
}
unsigned int Costmap2D::getSizeInCellsY() const
{
return size_y_;
}

double getSizeInMetersX() const;
double getSizeInMetersY() const;
	作用：使用米为单位进行x、y边长计算

double Costmap2D::getSizeInMetersX() const
{
return (size_x_ - 1 + 0.5) * resolution_;
}

double Costmap2D::getSizeInMetersY() const
{
return (size_y_ - 1 + 0.5) * resolution_;//-1+0.5是防止越界
}

double getOriginX() const;
double getOriginY() const;
	作用：获取原点的位置（x, y）

double getResolution() const;
	作用：获取地图的分辨率，就是按照什么样的规格进行对x y边长进行刻度

double Costmap2D::getOriginX() const
{
return origin_x_;
}

double Costmap2D::getOriginY() const
{
return origin_y_;
}

double Costmap2D::getResolution() const
{
return resolution_;
}

void setDefaultValue(unsigned char c)
{
default_value_ = c;
}

作用：设定代价值的默认值

unsigned char getDefaultValue() //代价值
{
return default_value_;
}

作用：获取代价值的默认值

bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value); //设定值
	作用：设定多边形，设定代价值，机器人轮廓

bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i)
{
MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
{
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}

std::vector<MapLocation> polygon_cells;

// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);

// set the cost of those cells
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
{
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
}
return true;
}

void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
	作用：获取构成多边形轮廓的栅格地图，地图坐标中要栅格化的多边形，多边形单元格将设置为多边形轮廓中包含的单元格

void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
for (unsigned int i = 0; i < polygon.size() - 1; ++i)
{
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
}
if (!polygon.empty())
{
unsigned int last_index = polygon.size() - 1;
// we also need to close the polygon by going from the last point to the first
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
}
}

void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
	作用：获取填充凸多边形的代价地图，地图坐标中要栅格化的多边形，多边形单元格将设置为填充多边形的单元格

void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
// we need a minimum polygon of a triangle  原理就是两点无法形成图形
if (polygon.size() < 3)
return;

// first get the cells that make up the outline of the polygon
polygonOutlineCells(polygon, polygon_cells);

// quick bubble sort to sort points by x
MapLocation swap;
unsigned int i = 0;
while (i < polygon_cells.size() - 1)
{
if (polygon_cells[i].x > polygon_cells[i + 1].x)
{
swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap;

if (i > 0)
--i;
}
else
++i;
}

i = 0;
MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;

// walk through each column and mark cells inside the polygon
for (unsigned int x = min_x; x <= max_x; ++x)
{
if (i >= polygon_cells.size() - 1)
break;

if (polygon_cells[i].y < polygon_cells[i + 1].y)
{
min_pt = polygon_cells[i];
max_pt = polygon_cells[i + 1];
}
else
{
min_pt = polygon_cells[i + 1];
max_pt = polygon_cells[i];
}

i += 2;
while (i < polygon_cells.size() && polygon_cells[i].x == x)
{
if (polygon_cells[i].y < min_pt.y)
min_pt = polygon_cells[i];
else if (polygon_cells[i].y > max_pt.y)
max_pt = polygon_cells[i];
++i;
}

MapLocation pt;
// loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
{
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}
}
}

virtual void updateOrigin(double new_origin_x, double new_origin_y);
	作用：将costmap的原点移到新位置（new_origin_x， new_origin_y ），尽可能保留数据

bool saveMap(std::string file_name);
	作用：存储地图，输入文件名称

void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,double origin_y);
	作用：重置地图，输入边长 size_x、 size_y，分辨率 resolution，原点（origin_x，origin_y）原地图中的数据会消失

void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
double origin_x, double origin_y)
{
size_x_ = size_x;
size_y_ = size_y;
resolution_ = resolution;
origin_x_ = origin_x;
origin_y_ = origin_y;

initMaps(size_x, size_y);

// reset our maps to have no information
resetMaps();
}

void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
	作用：重置地图，输入（x0, y0） (xn, yn) ，起点与终点（重置部分地图）

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
boost::unique_lock<mutex_t> lock(*(access_));
unsigned int len = xn - x0;
for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

unsigned int cellDistance(double world_dist);
	作用：输入世界距离（世界坐标的距离，将其转换为单元格），输出均等距离

typedef boost::recursive_mutex mutex_t;//提供typedef以便于将来的代码维护
	mutex_t* getMutex()
	{
		return access_;
	}

	作用：获取线程的参数，对于处理地图信息应该有锁进行

protected:

template<typename data_type>
	void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
	{
//我们第一次需要计算每一个地图开始的点，起点，指针
data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);

// 现在，我们将拷贝源地图映射目标地图中
for (unsigned int i = 0; i < region_size_y; ++i)
{
memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
sm_index += sm_size_x;
dm_index += dm_size_x;
}
}

	作用：将源映射的区域复制到目标映射中

virtual void deleteMaps();
	作用：删除地图指针，以及该指针中代表的代价值

	void Costmap2D::deleteMaps()
{
// clean up data
boost::unique_lock<mutex_t> lock(*access_);
delete[] costmap_;
costmap_ = NULL;
}

virtual void resetMaps();
	作用：对于未知空间，重置代价地图和静态地图

void Costmap2D::resetMaps()
{
boost::unique_lock<mutex_t> lock(*access_);
memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

virtual void initMaps(unsigned int size_x, unsigned int size_y);
	作用：初始化地图，输入地图的边长x, y

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
boost::unique_lock<mutex_t> lock(*access_);
delete[] costmap_;
costmap_ = new unsigned char[size_x * size_y];
}

template<class ActionType> //地图信息代价值计算，采用bresenham算法
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length = UINT_MAX)//UINT_MAX 是一个特别大的数
{
int dx = x1 – x0;//两点之间
int dy = y1 - y0;

unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);

int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_; 

unsigned int offset = y0 * size_x_ + x0;  // 起点的下标值

//我们需要根据线条的最大长度来选择我们主要维度的比例
double dist = hypot(dx, dy);//sqrt(x^2 + y^2)计算总体距离
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);

// 如果x比较长
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}

// 如果y比较长
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}

作用：计算代价值，通过bresenham算法

private:
template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset, unsigned int max_length)
{
unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i)
{
at(offset);
offset += offset_a;
error_b += abs_db;
if ((unsigned int)error_b >= abs_da)
{
offset += offset_b;
error_b -= abs_da;
}
}
at(offset);
}

作用：bresenham算法具体实现，输入两点xy方向绝对值，初始化误差，增加值，起点的下标值，长度总值     输出：两点之间进行补点
推导公式：斜率k = (abs_db/abs_da)
每一步增加：i× (abs_db/abs_da)>0.5
所以：i×abs_db+0.5abs_da>abs_da
进行误差减

inline int sign(int x)
{
return x > 0 ? 1.0 : -1.0;
}

作用：输出比较后的位置

protected:

class MarkCell
{
public:
	//构造函数
	MarkCell(unsigned char* costmap, unsigned char value) :costmap_(costmap), value_(value){}

inline void operator()(unsigned int offset)
{
	costmap_[offset] = value_;
}
	作用：对（）进行重载，然后表示地图值记录，每一个值就是一个下标值
private:
unsigned char* costmap_;
unsigned char value_;
};
作用：MarkCell类，表示地图中代价值

class PolygonOutlineCells //输出图形轮廓线，包括地图，指针以及轮廓中位置点
{
public:
	PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) :costmap_(costmap), char_map_(char_map), cells_(cells){}

// 用这个进行记录数据，应用于bresenham算法中，以及轮廓填充过程
	inline void operator()(unsigned int offset)
	{
		MapLocation loc;
		costmap_.indexToCells(offset, loc.x, loc.y);
		cells_.push_back(loc);
	}

参考：
1、代码来源：Turtlebot3-navigation
2、参考：https://www.cnblogs.com/zjiaxing/p/5543386.html  bresenham算法原理
3、https://www.cnblogs.com/flyinggod/p/9083094.html