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()); 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]); }
bool ElevationMap::publishFusedElevationMap() { if (!hasFusedMapSubscribers()) return false; boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_); GridMap fusedMapCopy = fusedMap_; scopedLock.unlock(); fusedMapCopy.add("uncertainty_range", fusedMapCopy.get("upper_bound") - fusedMapCopy.get("lower_bound")); grid_map_msgs::GridMap message; GridMapRosConverter::toMessage(fusedMapCopy, message); elevationMapFusedPublisher_.publish(message); ROS_DEBUG("Elevation map (fused) has been published."); return true; }
TEST(GridMapCvProcessing, changeResolution) { // Create grid map. GridMap mapIn; mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); mapIn.add("layer", 1.0); for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd(); ++iterator) { mapIn.at("layer", *iterator) = 2.0; } // Change resolution. GridMap mapOut; EXPECT_TRUE(GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1)); // Check data. EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all()); EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner. EXPECT_EQ(mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center. }