机器人导航地图——costmap局部地图滚动costmap_.updateOrigin理解

1.Introduction

if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
    //std::cout << "new_origin_x = " << new_origin_x << std::endl;
    //std::cout << "new_origin_y = " << new_origin_y << std::endl;
  }

代价地图的一个参数，表示该代价地图的world坐标系原点(origin_x_, origin_y_)【代价地图原点都在地图的左下角】是否要随着机器人移动而改变。换句话说，world坐标系原点要时刻“跟随”机器人中心，并保持着一个固定偏移(-getSizeInMetersX() / 2, -getSizeInMetersY() / 2)。rolling_window是true时，一旦机器人移动了，要以机器人中心坐标为参数调用updateOrigin修改出代价地图原点。对全局代价地图，world坐标系原点是机器人起始点，因而值是false。对局部代价地图，是一个以机器人中心为中心、半径1.5米的矩形，因而值是true。

简单来讲，地图的坐标原点和机器人绑定在一起，他和机器人的关系“保持着一个固定偏移(-getSizeInMetersX() / 2, -getSizeInMetersY() / 2)”。

2.Content

void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{
  // project the new origin into the grid
  int cell_ox, cell_oy;
  cell_ox = int((new_origin_x - origin_x_) / resolution_);
  cell_oy = int((new_origin_y - origin_y_) / resolution_);
  
  std::cout << "new_origin_x = " << new_origin_x << std::endl;
  std::cout << "new_origin_y = " << new_origin_y << std::endl;
 
  std::cout << "origin_x_ = " << origin_x_ << std::endl;
  std::cout << "origin_y_ = " << origin_y_ << std::endl;

  std::cout << "cell_ox:" << cell_ox << std::endl;
  std::cout << "cell_oy:" << cell_oy << std::endl;

  // compute the associated world coordinates for the origin cell
  // because we want to keep things grid-aligned
  double new_grid_ox, new_grid_oy;
  new_grid_ox = origin_x_ + cell_ox * resolution_;
  new_grid_oy = origin_y_ + cell_oy * resolution_;

  // To save casting from unsigned int to int a bunch of times
  int size_x = size_x_;
  int size_y = size_y_;
  std::cout << "size_x:" << size_x << std::endl;
  std::cout << "size_y:" << size_y << std::endl;
  // we need to compute the overlap of the new and existing windows
  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
  lower_left_x = min(max(cell_ox, 0), size_x);
  lower_left_y = min(max(cell_oy, 0), size_y);
  upper_right_x = min(max(cell_ox + size_x, 0), size_x);
  upper_right_y = min(max(cell_oy + size_y, 0), size_y);

  unsigned int cell_size_x = upper_right_x - lower_left_x;
  unsigned int cell_size_y = upper_right_y - lower_left_y;

  // we need a map to store the obstacles in the window temporarily
  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];

  // copy the local window in the costmap to the local map
  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);

  // now we'll set the costmap to be completely unknown if we track unknown space
  resetMaps();

  // update the origin with the appropriate world coordinates
  origin_x_ = new_grid_ox;
  origin_y_ = new_grid_oy;

  // compute the starting cell location for copying data back in
  int start_x = lower_left_x - cell_ox;
  int start_y = lower_left_y - cell_oy;

  // now we want to copy the overlapping information back into the map, but in its new location
  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);

  // make sure to clean up
  delete[] local_map;
}

上述的updateOrigin代码执行两个任务。一是重新计算原点origin_x_/y_，方法是微调参数new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除。二是生成新的区域地图，新地图尺寸不变，前后两次重叠区域保持原值，其它部分则填上默认值default_value_。

这段代码是一个Costmap2D类的方法，用于更新地图坐标原点。
首先，将新的原点坐标转换为格子坐标，即将地图上的坐标值转换为在网格中的索引值。然后，根据格子坐标计算出与原点单元格关联的世界坐标。这里需要确保世界坐标是与网格对齐的。为了避免多次进行无符号整型到整型的强制转换，将地图的大小保存到局部变量中。接下来，计算新窗口与现有窗口的重叠部分。找出重叠部分的左下角和右上角的格子坐标。然后，根据重叠部分计算出局部地图的大小，并创建一个用于存储障碍物的局部地图数组。将地图中的重叠窗口的数据拷贝到局部地图中。如果追踪未知空间，则将地图设置为完全未知。更新原点坐标为新的网格坐标。计算从局部地图拷贝数据到地图中的起始位置。将重叠部分数据从局部地图拷贝回地图中。最后进行清理，释放局部地图内存。

用两个copyMapRegion以及restMaps，生成新的区域地图：
（1）第一个copyMapRegion把蓝色、红色交叉部分（白框）复制到新建的临时内存块local_map。复制时，蓝色地图图像是源，即costmap_，costmap_的跨距size_x_；local_map是目标，它的宽度等于源中要复制的宽度cell_size_x，因为每栅格值一个字节，这个宽度也等于跨距。理解copyMapRegion参数时，注意这是world坐标系，图像原点在左下角。复制出重叠块后，用restMaps把整个costmap_置为默认值default_value_。
（2）第二个copyMapRegion把暂存在local_map中的数据复制回costmap_，此时costmap_已指到红色块。复制时，local_map变成了源，costmap_变成了目标。

综上所述，经过updateOrigin后，前后两次重叠区域保持原值，其它部分则填上默认default_value_。

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)
    {
      // we'll first need to compute the starting points for each map
      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);

      // now, we'll copy the source map into the destination map
      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;
      }
    }

上述代码是copyMapRegion的函数实现，这段代码是一个模板函数 copyMapRegion 的实现，用于将源地图的一个区域拷贝到目标地图中。
函数的参数说明如下：

• source_map：源地图的指针，指向需要拷贝的数据区域的起始位置。
• sm_lower_left_x 和 sm_lower_left_y：源地图区域的左下角格子坐标。
• sm_size_x：源地图的宽度。
• dest_map：目标地图的指针，指向拷贝数据的目标位置。
• dm_lower_left_x 和 dm_lower_left_y：目标地图区域的左下角格子坐标。
• dm_size_x：目标地图的宽度。
• region_size_x 和 region_size_y：要拷贝的源地图区域的宽度和高度。
  该函数的实现逻辑为：

1. 首先，根据源地图区域的左下角格子坐标和地图的宽度，计算出源地图的起始点 sm_index 和目标地图的起始点 dm_index。
2. 使用 memcpy 函数，将源地图区域的数据拷贝到目标地图区域。
   • memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)) 表示将 region_size_x 个数据元素从 sm_index 指向的源地图区域拷贝到 dm_index 指向的目标地图区域。
3. 通过每次增加 sm_size_x 和 dm_size_x，将 sm_index 和 dm_index 移动到下一行的起始位置，然后继续执行拷贝操作，直到拷贝完所有行。
   总之，这段代码实现了将源地图中一个指定区域的数据拷贝到目标地图的功能。该函数是通过指针和数组操作来实现数据的拷贝，相比遍历每个元素进行单独赋值的方式，可以提高数据拷贝的效率。

Reference
1. world/map坐标系、updateOrigin、footprintCost