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 ElevationChangeDetection::computeElevationChange(grid_map::GridMap& elevationMap)
{
  elevationMap.add("elevation_change", elevationMap.get(layer_));
  elevationMap.clear("elevation_change");

  for (GridMapIterator iterator(elevationMap);
      !iterator.isPastEnd(); ++iterator) {
    // Check if elevation map has valid value
    if (!elevationMap.isValid(*iterator, layer_)) continue;
    double height = elevationMap.at(layer_, *iterator);

    // Get the ground truth height
    Vector2d position, groundTruthPosition;
    Array2i groundTruthIndex;
    elevationMap.getPosition(*iterator, position);
    groundTruthMap_.getIndex(position, groundTruthIndex);
    if (!groundTruthMap_.isValid(groundTruthIndex, layer_)) continue;
    double groundTruthHeight = groundTruthMap_.at(layer_, groundTruthIndex);

    // Add to elevation change map
    double diffElevation = std::abs(height - groundTruthHeight);
    if (diffElevation <= threshold_) continue;
    elevationMap.at("elevation_change", *iterator) = diffElevation;
  }
}
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);
	}
Example #5
0
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;
}
Example #6
0
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);
}
Example #7
0
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::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);
    }
  }
}
Example #10
0
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_);
}