程式碼解析之grid_map庫grid_map_core工程(二)——move功能
阿新 • • 發佈:2019-02-06
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]); }