Costmap 2 D類解析

**    ：**
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<: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& polygon, std::vector& 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& polygon, std::vector& 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_;
	}

	  ：       ，              

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;
}

  ：        

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