bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start, const Position& end, Index& index) { Position newStart = start; Vector direction = (end - start).normalized(); while (!gridMap.getIndex(newStart, index)) { newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction; if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon()) return false; } return true; }
void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers, grid_map_msgs::GridMap& message) { message.info.header.stamp.fromNSec(gridMap.getTimestamp()); message.info.header.frame_id = gridMap.getFrameId(); message.info.resolution = gridMap.getResolution(); message.info.length_x = gridMap.getLength().x(); message.info.length_y = gridMap.getLength().y(); message.info.pose.position.x = gridMap.getPosition().x(); message.info.pose.position.y = gridMap.getPosition().y(); message.info.pose.position.z = 0.0; message.info.pose.orientation.x = 0.0; message.info.pose.orientation.y = 0.0; message.info.pose.orientation.z = 0.0; message.info.pose.orientation.w = 1.0; message.layers = layers; message.basic_layers = gridMap.getBasicLayers(); message.data.clear(); for (const auto& layer : layers) { std_msgs::Float32MultiArray dataArray; matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray); message.data.push_back(dataArray); } message.outer_start_index = gridMap.getStartIndex()(0); message.inner_start_index = gridMap.getStartIndex()(1); }
void ElevationVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameElevation, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("color"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at(typeNameElevation, buffIndex); if(std::isnan(elevation)) continue; const float color = haveColor ? map.at(typeNameColor, buffIndex) : lowerValueBound; // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { std_msgs::ColorRGBA markerColor = grid_map_visualization::color_utils::interpolateBetweenColors( color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); marker_.colors.push_back(markerColor); } } } markerPublisher_.publish(marker_); }
void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points, const std::string &in_grid_layer_name, const int in_layer_background_value, const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value, const std::string &in_tf_target_frame, const std::string &in_tf_source_frame, const tf::TransformListener &in_tf_listener) { if(!out_grid_map.exists(in_grid_layer_name)) { out_grid_map.add(in_grid_layer_name); } out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value); cv::Mat original_image; grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map, in_grid_layer_name, CV_8UC1, in_layer_min_value, in_layer_max_value, original_image); cv::Mat filled_image = original_image.clone(); tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener); // calculate out_grid_map position grid_map::Position map_pos = out_grid_map.getPosition(); double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x(); double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y(); for (const auto &points : in_area_points) { std::vector<cv::Point> cv_points; for (const auto &p : points) { // transform to GridMap coordinate geometry_msgs::Point tf_point = TransformPoint(p, tf); // coordinate conversion for cv image double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution(); double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution(); cv_points.emplace_back(cv::Point(cv_x, cv_y)); } cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color)); } // convert to ROS msg grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image, in_grid_layer_name, out_grid_map, in_layer_min_value, in_layer_max_value); }
bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end) { start_ = start; end_ = end; mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); bufferStartIndex_ = gridMap.getStartIndex(); initializeIterationParameters(); return true; }
void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, float lowerThreshold, float upperThreshold, nav_msgs::GridCells& gridCells) { gridCells.header.frame_id = gridMap.getFrameId(); gridCells.header.stamp.fromNSec(gridMap.getTimestamp()); gridCells.cell_width = gridMap.getResolution(); gridCells.cell_height = gridMap.getResolution(); for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { if (!gridMap.isValid(*iterator, layer)) continue; if (gridMap.at(layer, *iterator) >= lowerThreshold && gridMap.at(layer, *iterator) <= upperThreshold) { Position position; gridMap.getPosition(*iterator, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); gridCells.cells.push_back(point); } } }
PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon) : polygon_(polygon) { mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); bufferStartIndex_ = gridMap.getStartIndex(); Index submapStartIndex; Size submapBufferSize; findSubmapParameters(polygon, submapStartIndex, submapBufferSize); internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); if(!isInside()) ++(*this); }
SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius) : center_(center), radius_(radius), distance_(0) { radiusSquare_ = radius_ * radius_; mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); gridMap.getIndex(center_, indexCenter_); nRings_ = std::ceil(radius_ / resolution_); if (checkIfIndexWithinRange(indexCenter_, bufferSize_)) pointsRing_.push_back(indexCenter_); else generateRing(); }
bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid, const std::string& layer, grid_map::GridMap& gridMap) { const Size size(occupancyGrid.info.width, occupancyGrid.info.height); const double resolution = occupancyGrid.info.resolution; const Length length = resolution * size.cast<double>(); const string& frameId = occupancyGrid.header.frame_id; Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y); // Different conventions of center of map. position += 0.5 * length.matrix(); const auto& orientation = occupancyGrid.info.origin.orientation; if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0 && orientation.z == 0 && orientation.w == 0)) { ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation."); ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation); return false; } if (size.prod() != occupancyGrid.data.size()) { ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height."); return false; } if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution || (gridMap.getLength() != length).any() || gridMap.getPosition() != position || gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) { gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec()); gridMap.setFrameId(frameId); gridMap.setGeometry(length, resolution, position); } // Reverse iteration is required because of different conventions // between occupancy grid and grid map. grid_map::Matrix data(size(0), size(1)); for (std::vector<int8_t>::const_reverse_iterator iterator = occupancyGrid.data.rbegin(); iterator != occupancyGrid.data.rend(); ++iterator) { size_t i = std::distance(occupancyGrid.data.rbegin(), iterator); data(i) = *iterator != -1 ? *iterator : NAN; } gridMap.add(layer, data); return true; }
void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer, float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid) { occupancyGrid.header.frame_id = gridMap.getFrameId(); occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp()); occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map. occupancyGrid.info.resolution = gridMap.getResolution(); occupancyGrid.info.width = gridMap.getSize()(0); occupancyGrid.info.height = gridMap.getSize()(1); Position positionOfOrigin; getPositionOfDataStructureOrigin(gridMap.getPosition(), gridMap.getLength(), positionOfOrigin); occupancyGrid.info.origin.position.x = positionOfOrigin.x(); occupancyGrid.info.origin.position.y = positionOfOrigin.y(); occupancyGrid.info.origin.position.z = 0.0; occupancyGrid.info.origin.orientation.x = 0.0; occupancyGrid.info.origin.orientation.y = 0.0; occupancyGrid.info.origin.orientation.z = 1.0; // yes, this is correct. occupancyGrid.info.origin.orientation.w = 0.0; occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height); // Occupancy probabilities are in the range [0,100]. Unknown is -1. const float cellMin = 0; const float cellMax = 100; const float cellRange = cellMax - cellMin; for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); if (isnan(value)) value = -1; else value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; // Occupancy grid claims to be row-major order, but it does not seem that way. // http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html. unsigned int index = get1dIndexFrom2dIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); occupancyGrid.data[index] = value; } }
void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer, float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid) { occupancyGrid.header.frame_id = gridMap.getFrameId(); occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp()); occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map. occupancyGrid.info.resolution = gridMap.getResolution(); occupancyGrid.info.width = gridMap.getSize()(0); occupancyGrid.info.height = gridMap.getSize()(1); Position position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix(); occupancyGrid.info.origin.position.x = position.x(); occupancyGrid.info.origin.position.y = position.y(); occupancyGrid.info.origin.position.z = 0.0; occupancyGrid.info.origin.orientation.x = 0.0; occupancyGrid.info.origin.orientation.y = 0.0; occupancyGrid.info.origin.orientation.z = 0.0; occupancyGrid.info.origin.orientation.w = 1.0; size_t nCells = gridMap.getSize().prod(); occupancyGrid.data.resize(nCells); // Occupancy probabilities are in the range [0,100]. Unknown is -1. const float cellMin = 0; const float cellMax = 100; const float cellRange = cellMax - cellMin; for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); if (isnan(value)) value = -1; else value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); // Reverse cell order because of different conventions between occupancy grid and grid map. occupancyGrid.data[nCells - index - 1] = value; } }
void SurfaceVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameSurface, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("class"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at("mu", buffIndex); const float& class_pred = map.at("class", buffIndex); int class_number=0; if(!std::isnan(class_pred)) { class_number=(int)class_pred; } if(std::isnan(elevation)) { continue; } //const float color = haveColor ? 16711680/class_pred : lowerValueBound; //ROS_INFO_STREAM("class_pred: "<<class_pred <<" int: "<<(int)class_pred); std_msgs::ColorRGBA surface_color; surface_color.a = 1.0; switch (class_number) { case 1: //long_grass surface_color.r = 0.0; surface_color.g = 1.0; surface_color.b = 0.0; break; case 3: //cobblestone surface_color.r = 0.62; surface_color.g = 0.32; surface_color.b = 0.17; break; case 4: //gravel surface_color.r = 0.2; surface_color.g = 0.2; surface_color.b = 0.2; break; default: surface_color.r = 1.0; surface_color.g = 1.0; surface_color.b = 1.0; break; } // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { // std_msgs::ColorRGBA markerColor = // grid_map_visualization::color_utils::interpolateBetweenColors( // color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); //marker_.colors.push_back(markerColor); marker_.colors.push_back(surface_color); } } } markerPublisher_.publish(marker_); }