沉默杀手HomeArchivesTagsCategories
Costmap2D 类
|路径规划代价地图|
Word count: 1.9k|Reading time: 8 min

Costmap2DROS, Costmap2D, LayeredCostmap三个类都属于命名空间costmap_2d，互相没有继承关系，而且都是独立的类

Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构. 它的定义在LayeredCostmap类的成员列表初始化里面，cell默认值default_value_的赋值在LayerdCostmap类的构造函数里。

三个类的关系图

地图图像原点在左下角，缺点是会增加理解难度，好处是在生成图像时，y可以 +1 增，符合真实世界中y向北表示增加。要画出图像时，须要上下颠倒，y往往要换成(map.info.height - y - 1)

代价地图的坐标系和索引

A 2D costmap class provides a mapping between points in the world and their associated "costs"

map坐标系原点在该区域左下角坐标(origin_x_, origin_y_)附近。具体是在(origin_x_ + resolution/2, origin_x_ + resolution/2)

在一次导航过程中，world坐标系只有一个，原点固定。map坐标则随着区域出现而出现。由于一个栅格表示一个resolutin X resolution面积的真实区域，栅格坐标对应的world坐标是在该栅格的中心点坐标。


看全局代价地图的各个参数:

1
2
3
4
5
6
7
8
9
int nx = global_costmap->getSizeInCellsX();   //2200
int ny = global_costmap->getSizeInCellsY();	  //1600	
double global_resolution = global_costmap->getResolution();  // 0.025

double origin_x = global_costmap->getOriginX();    // -1
double origin_y = global_costmap->getOriginY();    // -34.6

double sizeMeterX = global_costmap->getSizeInMetersX();     // 109.975
double sizeMeterY = global_costmap->getSizeInMetersY();     //  79.975

nx, ny就是pgm地图文件的像素尺寸，一共nx X ny个cell

global_resolution, origin_x, origin_y就是map.yaml中的resolution和origin参数

sizeMeterX 和 sizeMeterY 在rviz上用Measure工具测量一下地图的长宽即可得到


局部代价地图，yaml文件的参数是这样设置的:

1
2
3
width: 4.0
height: 4.0
resolution: 0.05

nx, ny都是80，也就是4/0.05, 80x80就是6400个cell

origin_x: 58, origin_y: 1.5 这里机器人的初值位置为(60, 3.5)，位于动态窗口的中心，计算就能得到这两个参数

sizeMeterX: 3.975000, sizeMeterY: 3.975000 其实就是4

这里会发现在全局代价地图的yaml里，没有设置width和height，因为它们是地图文件决定的，无需我们设置。而局部代价地图的动态窗口大小是我们决定的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
// 就是为 costmap_ 分配内存
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];
}

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

// 将 costmap_ 赋值为默认值
void Costmap2D::resetMaps()
{
    boost::unique_lock<mutex_t> lock(*access_);
    memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

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));
}
1
2
3
4
5
6
7
8
9
10
11
12
// 根据像素坐标，获得 costmap_ 数组的索引
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
    return my * size_x_ + mx;
}

// 根据 costmap_ 数组的索引，获得像素坐标
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
    my = index / size_x_;
    mx = index - (my * size_x_);
}

  • void Costmap2D::deleteMaps(): 释放 costmap_

  • bool saveMap(std::string file_name); 

1
2
3
4
5
6
// 输入世界距离（世界坐标的距离，将其转换为单元格），输出均等距离
unsigned int Costmap2D::cellDistance(double world_dist)
{
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}

代价地图保存为pgm格式。全局代价地图保存是这样的
saveMap.png
这里保存的地图图片，其实和OpenCV保存的一样。相当于真实的costmap先逆时针转180°，再对y轴取对称。但是像素值是相反的。局部代价地图打开是一片黑，应该是只有个窗口，看不出来了。

  • void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
1
2
3
4
5
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_;
}

把某个代价地图里的坐标转为世界坐标，常用于把机器人全局代价地图的坐标转为map坐标系的坐标。 在TebLocalPlannerROS::updateObstacleContainerWithCostmap()调用过: costmap_->mapToWorld(0, 0, wx, wy);

  • void worldToMap(double robot_x, double robot_y, int mx, int my) const
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// Convert from world coordinates to map coordinates
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;
}

把世界坐标转为某个代价地图里的坐标，最常用的是把机器人在map坐标系的坐标转为全局代价地图里的坐标，注意是转为像素坐标，不是base_link或常说的坐标系。 像素坐标系的原点是左下方的点 ，所以是跟size_x_和size_y_比较，这两个是地图的长宽，这样才能判断点是否出了代价地图的范围

1
2
3
4
5
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);
unsigned int mx, my;
costmap_ros_->getCostmap()->worldToMap(robot_pose->x(), robot_pose->y(), mx, my);

如果是局部代价地图，窗口边长4，分辨率0.05，那么窗口的像素边长就是80，这里的机器人的坐标 mx my 就在(40, 40)左右，因为它总在局部代价地图中心

1
2
3
4
5
if (!costmap_->worldToMap(x, y, mx, my)) {
    ROS_FATAL("The dimensions of costmap is too small to fully include robot's footprint");
    return;
}
unsigned char cost = costmap_->getCost(mx, my);

如果worldToMap返回false，那么是mx或my太大了，超出了代价地图的大小。如果不return，继续执行getCost会导致程序退出。还可以return -1，因为代价值是0~255。 有时返回false是因为 wx < originx || wy < originy 不明白原因

map坐标系坐标单元是栅格，只有整数才有意义。一般来说，只有在区域内的才有意义，即x范围是[0, size_x_-1]，y范围是[0, size_y_-1]。 这就涉及到worldToMapEnforceBounds 和 worldToMapNoBounds


  • unsigned char getCost (unsigned int mx, unsigned int my) const    获得costmap中某个栅格的代价值。 mx, my是cell的坐标

  • void setCost(unsigned int mx, unsigned int my, unsigned char cost)

设置代价地图中某个cell的代价值，在层的代码中使用很多

  • void setDefaultValue(unsigned char c)

语句只有default_value_ = c;，调用的地方只有LayeredCostmap构造函数

1
2
3
4
if (track_unknown)  // 默认false
  costmap_.setDefaultValue(255);
else
  costmap_.setDefaultValue(0);

  • unsigned char* getCharMap() const;     return A pointer to the underlying unsigned char array storing cost values     获取到这个地图中存储代价地图的指针，通过指针找到地图信息。主要在层的代码和ClearCostmapRecovery类中使用
1
2
3
4
unsigned char* Costmap2D::getCharMap() const
{
  return costmap_;
}

获得指定栅格周围的4个或8个栅格

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
/**
 * @brief Determine 4-connected neighbourhood of an input cell, checking for map edges
 * @param idx input cell index
 * @param costmap Reference to map data
 * @return neighbour cell indexes
 */
std::vector<unsigned int> nhood4(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 4-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out;
  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();
  if (idx > size_x_ * size_y_ - 1)
  {
    ROS_WARN("Evaluating nhood for offmap point");
    return out;
  }
  if (idx % size_x_ > 0) {
    out.push_back(idx - 1);
  }
  if (idx % size_x_ < size_x_ - 1) {
    out.push_back(idx + 1);
  }
  if (idx >= size_x_) {
    out.push_back(idx - size_x_);
  }
  if (idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + size_x_);
  }
  return out;
}

/**
 * @brief Determine 8-connected neighbourhood of an input cell, checking for map edges
 * @param idx input cell index
 * @param costmap Reference to map data
 * @return neighbour cell indexes
 */
std::vector<unsigned int> nhood8(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 8-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out = nhood4(idx, costmap);

  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();

  if (idx > size_x_ * size_y_ - 1) {
    return out;
  }

  if (idx % size_x_ > 0 && idx >= size_x_) {
    out.push_back(idx - 1 - size_x_);
  }
  if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx - 1 + size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
    out.push_back(idx + 1 - size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + 1 + size_x_);
  }
  return out;
}