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)); }
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. }
void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, const double heightClearance) { data_.clear(); resolution_ = gridMap.getResolution(); position_ = gridMap.getPosition(); size_ = gridMap.getSize(); Matrix map = gridMap.get(layer); // Copy! float minHeight = map.minCoeffOfFinites(); if (!std::isfinite(minHeight)) minHeight = lowestHeight_; float maxHeight = map.maxCoeffOfFinites(); if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_; const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option). for (size_t i = 0; i < map.size(); ++i) { if (std::isnan(map(i))) map(i) = valueForEmptyCells; } // Height range of the signed distance field is higher than the max height. maxHeight += heightClearance; Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_; Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols()); std::vector<Matrix> sdf; zIndexStartHeight_ = minHeight; // Calculate signed distance field from bottom. for (float h = minHeight; h < maxHeight; h += resolution_) { Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h; Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1; Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField); Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField); Matrix sdf2d; // If 2d sdfObstacleFree calculation failed, neglect this SDF // to avoid extreme small distances (-INF). if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle; else sdf2d = sdfObstacle - sdfObstacleFree; sdf2d *= resolution_; for (size_t i = 0; i < sdfElevationAbove.size(); ++i) { if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i); else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_; if (sdf2d(i) == 0) sdfLayer(i) = h - map(i); else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h)); else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i)); } data_.push_back(sdfLayer); } }
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation) : center_(center) { semiAxisSquare_ = (0.5 * length).square(); double sinRotation = sin(rotation); double cosRotation = cos(rotation); transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation; mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); bufferStartIndex_ = gridMap.getStartIndex(); Index submapStartIndex; Index submapBufferSize; findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize); internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); if(!isInside()) ++(*this); }
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)); }