costmap_2d详解7：costmap_2d.cpp分析

关键变量

  unsigned int size_x_;  //像素坐标系表示的坐标
  unsigned int size_y_;
  double resolution_;
  double origin_x_;
  double origin_y_;
  unsigned char* costmap_;
  unsigned char default_value_;

 

关键函数：

/*默认使用的是这个构造函数*/

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

 

/*

* 作用：重新分配size_x*size_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 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];
}

 

/*返回costmap 地图指针

* 必须要分清楚该指针是指向master costmap

* 还是其他分层的costmap

*/

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

/*获取cosmap地图某个点的cost值

*/

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

/*设置cosmap地图某个点的cost值

*/

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

 

/*static layer 中mx,my 为0 ，第二次为地图的宽width_和高height_

*/

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{//+0.5 为了去除边界，防止计算到地图外面，(这里map 是指像素坐标系，单位为int，
  //world 是指/map世界坐标系)
  wx = origin_x_ + (mx + 0.5) * resolution_;
  wy = origin_y_ + (my + 0.5) * resolution_;
}

/*

将世界坐标系的点(map 坐标系，在启动建图时生成)转换成图像坐标系(图像左下角)下的点(以像素表示)

*/

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 Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{
  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);
}

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

 

/*

*/

void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)

 

/*

*返回costmap 的宽度，以像素表示

*/

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

/*

*返回costmap 的高度，以像素表示

*/

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

/*

*返回costmap 的宽度，以距离米表示

* -1+0.5是防止越界

*/

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

/*

*返回costmap 的高度，以距离米表示

* -1+0.5是防止越界

*/

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

 

/*

* 返回costmap 地图的左下角x坐标,global_costmap与local_costmap 此点

* 不一样

*/

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

/*

* 返回costmap 地图的左下角y坐标,global_costmap与local_costmap 此点

* 不一样

*/

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

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

 

Costmap_layer.h 和costmap_layer.cpp

void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,int max_i, int max_j)
{
  if (!enabled_)
    return;

  //获取layer_costmap 某一层地图
  unsigned char* master = master_grid.getCharMap();

  //索引值，获取到costmap 地图宽度，以像素坐标系为参考的值
  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++)
    {
      //用static costmap 地图去更新master costmap 地图
      master[it] = costmap_[it];
      it++;
    }
  }
}

/*

* 覆盖的方式更新

*/

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