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]);
}
void PotentialField::init() {
ROS_INFO("Created map");
map_.setFrameId("/potential_field_link");
map_.setGeometry(Length(map_x_size_, map_y_size_), map_resolution_);
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("obstacle_field", *it) = 0.0;
map_.at("target_waypoint_field", *it) = 0.0;
map_.at("vscan_points_field", *it) = 0.0;
map_.at("potential_field", *it) = 0.0;
}
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
map_.getLength().x(), map_.getLength().y(), map_.getSize()(0),
map_.getSize()(1));
}
void populateMap(GridMap &map, string layer,string file_path, float scale,float res)
{
cv_bridge::CvImage cv_image;
Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE);
cv::Size img_size= img.size();
cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC);
map.setGeometry(Length(img.rows*res,img.cols*res),res);
for (GridMapIterator it(map); !it.isPastEnd(); ++it)
{
Position position;
map.getPosition(*it, position);
int x = (img.rows/2) + position.x()/res+res/2.0;
int y = (img.cols/2) + position.y()/res+res/2.0;
map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0;
}
map.setTimestamp(ros::Time::now().toNSec());
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1));
}
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.
}
