1. 程式人生 > >程式碼解析之grid_map庫grid_map_core工程(二)——move功能

程式碼解析之grid_map庫grid_map_core工程(二)——move功能

         grid_map庫的說明中,強調了因為該庫採用了circular buffer的資料結構,使得地圖移動時無需重新分配記憶體空間、無需移動和複製記憶體資料,即可儲存新的地圖資料,大大提高了效率。本文主要分析該功能的實現原理——move。

標頭檔案中宣告如下:

    /*!
       * Move the grid map w.r.t. to the grid map frame. Use this to move the grid map
       * boundaries without moving the grid map data. Takes care of all the data handling,
       * such that the grid map data is stationary in the grid map frame.
       * Note: For a comparison between the `setPosition` and the `move` method,
       * see the `move_demo_node.cpp` file of the `grid_map_demos` package.
       * @param position the new location of the grid map in the map frame.
       * @param newRegions the regions of the newly covered / previously uncovered regions of the buffer.
       * @return true if map has been moved, false otherwise.
       */
      bool move(const Position& position, std::vector<BufferRegion>& newRegions);

原始檔中定義和註釋如下:

//move過程沒有記憶體資料的移動和拷貝,只有部分割槽域的記憶體資料的清理
//move是由map center的移動引起的。map是有限定大小的,可以理解為硬體系統只能觀測到這麼大的範圍,所以只能建立這麼大的map,
//當發生移動後,原先在觀測範圍(map)內的內容現在觀測不到了,所以要在記憶體中清除掉。
//而原來本來就觀測到並儲存了的資料,依然還在原先的位置。
bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions)
{
  Index indexShift;
  Position positionShift = position - position_;
  getIndexShiftFromPositionShift(indexShift, positionShift, resolution_);
  Position alignedPositionShift;
  getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_);

  // Delete fields that fall out of map (and become empty cells).
  for (int i = 0; i < indexShift.size(); i++) {
    if (indexShift(i) != 0) {
      if (abs(indexShift(i)) >= getSize()(i)) {
        // Entire map is dropped.
        clearAll();
        newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined));
      } else {
        // Drop cells out of map.
        //區分map的移動方向
        int sign = (indexShift(i) > 0 ? 1 : -1);
        //找出從記憶體塊中要清除的資料區域的首尾位置
        int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0);
        int endIndex = startIndex - sign + indexShift(i);
        int nCells = abs(indexShift(i));
        //從左向右處理
        int index = (sign > 0 ? startIndex : endIndex);
        wrapIndexToRange(index, getSize()(i));

        if (index + nCells <= getSize()(i)) {
          // One region to drop.
          if (i == 0) {
            clearRows(index, nCells);
            newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(index, nCells);
            newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined));
          }
        } else {
          // Two regions to drop.
          int firstIndex = index;
          int firstNCells = getSize()(i) - firstIndex;
          if (i == 0) {
            clearRows(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined));
          }

          int secondIndex = 0;
          int secondNCells = nCells - firstNCells;
          if (i == 0) {
            clearRows(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
          }
        }
      }
    }
  }

  // Update information.
  startIndex_ += indexShift;
  wrapIndexToRange(startIndex_, getSize());
  position_ += alignedPositionShift;

  // Check if map has been moved at all.
  return (indexShift.any() != 0);
}

        注:什麼情況下會發生two regions to drop還不明白?

        結合自帶的test檔案,舉個例子:

TEST(GridMap, Move)
{
  GridMap map;
  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
  map.add("layer", 0.0);
  map.setBasicLayers(map.getLayers());
  std::vector<BufferRegion> regions;
  map.move(Position(-3.0, -2.0), regions);
  Index startIndex = map.getStartIndex();

  EXPECT_EQ(3, startIndex(0));
  EXPECT_EQ(2, startIndex(1));

  EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map.
  EXPECT_TRUE(map.isValid(Index(3, 2)));
  EXPECT_FALSE(map.isValid(Index(2, 2)));
  EXPECT_FALSE(map.isValid(Index(3, 1)));
  EXPECT_TRUE(map.isValid(Index(7, 4)));

  EXPECT_EQ(2, regions.size());
  //2個region的區域有重合的地方
  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
  EXPECT_EQ(3, regions[0].getSize()[0]);
  EXPECT_EQ(5, regions[0].getSize()[1]);
  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
  EXPECT_EQ(8, regions[1].getSize()[0]);
  EXPECT_EQ(2, regions[1].getSize()[1]);
}