bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
  if (!isActive()) return true;

  for (const auto& type : types_) {
    if (!map.exists(type)) {
      ROS_WARN_STREAM(
          "VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
      return false;
    }
  }

  // Set marker info.
  marker_.header.frame_id = map.getFrameId();
  marker_.header.stamp.fromNSec(map.getTimestamp());

  // Clear points.
  marker_.points.clear();
  marker_.colors.clear();

  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
  {
    if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
    geometry_msgs::Vector3 vector;
    vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set
    vector.y = map.at(types_[1], *iterator);
    vector.z = map.at(types_[2], *iterator);

    Eigen::Vector3d position;
    map.getPosition3(positionLayer_, *iterator, position);
    geometry_msgs::Point startPoint;
    startPoint.x = position.x();
    startPoint.y = position.y();
    startPoint.z = position.z();
    marker_.points.push_back(startPoint);

    geometry_msgs::Point endPoint;
    endPoint.x = startPoint.x + scale_ * vector.x;
    endPoint.y = startPoint.y + scale_ * vector.y;
    endPoint.z = startPoint.z + scale_ * vector.z;
    marker_.points.push_back(endPoint);

    marker_.colors.push_back(color_); // Each vertex needs a color.
    marker_.colors.push_back(color_);
  }

  publisher_.publish(marker_);
  return true;
}
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;
  }
}
bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
  if (markerPublisher_.getNumSubscribers () < 1) return true;

  // Set marker info.
  marker_.header.frame_id = map.getFrameId();
  marker_.header.stamp.fromNSec(map.getTimestamp());

  // Clear points.
  marker_.points.clear();
  marker_.colors.clear();

  for (grid_map_lib::GridMapIterator iterator(map); !iterator.isPassedEnd(); ++iterator)
  {
    if (!map.isValid(*iterator) || !map.isValid(*iterator, types_)) continue;

    geometry_msgs::Vector3 vector;
    vector.x = map.at(types_[0], *iterator);
    vector.y = map.at(types_[1], *iterator);
    vector.z = map.at(types_[2], *iterator);

    Eigen::Vector3d position;
    map.getPosition3d(positionType_, *iterator, position);
    geometry_msgs::Point startPoint;
    startPoint.x = position.x();
    startPoint.y = position.y();
    startPoint.z = position.z();
    marker_.points.push_back(startPoint);

    geometry_msgs::Point endPoint;
    endPoint.x = startPoint.x + scale_ * vector.x;
    endPoint.y = startPoint.y + scale_ * vector.y;
    endPoint.z = startPoint.z + scale_ * vector.z;
    marker_.points.push_back(endPoint);

    marker_.colors.push_back(color_); // Each vertex needs a color.
    marker_.colors.push_back(color_);
  }

  markerPublisher_.publish(marker_);
  return true;
}
示例#4
0
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);
    }
  }
}
void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
                                       const std::vector<std::string>& layers,
                                       const std::string& pointLayer,
                                       sensor_msgs::PointCloud2& pointCloud)
{
  // Header.
  pointCloud.header.frame_id = gridMap.getFrameId();
  pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
  pointCloud.is_dense = false;

  // Fields.
  std::vector <std::string> fieldNames;

  for (const auto& layer : layers) {
    if (layer == pointLayer) {
      fieldNames.push_back("x");
      fieldNames.push_back("y");
      fieldNames.push_back("z");
    } else if (layer == "color") {
      fieldNames.push_back("rgb");
    } else {
      fieldNames.push_back(layer);
    }
  }

  pointCloud.fields.clear();
  pointCloud.fields.reserve(fieldNames.size());
  int offset = 0;

  for (auto& name : fieldNames) {
    sensor_msgs::PointField pointField;
    pointField.name = name;
    pointField.count = 1;
    pointField.datatype = sensor_msgs::PointField::FLOAT32;
    pointField.offset = offset;
    pointCloud.fields.push_back(pointField);
    offset = offset + pointField.count * 4;  // 4 for sensor_msgs::PointField::FLOAT32
  }

  // Resize.
  size_t maxNumberOfPoints = gridMap.getSize().prod();
  pointCloud.height = 1;
  pointCloud.width = maxNumberOfPoints;
  pointCloud.point_step = offset;
  pointCloud.row_step = pointCloud.width * pointCloud.point_step;
  pointCloud.data.resize(pointCloud.height * pointCloud.row_step);

  // Points.
  std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
  for (auto& name : fieldNames) {
    fieldIterators.insert(
        std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
            name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
  }

  GridMapIterator mapIterator(gridMap);
  const bool hasBasicLayers = gridMap.getBasicLayers().size() > 0;

  size_t realNumberOfPoints = 0;
  for (size_t i = 0; i < maxNumberOfPoints; ++i) {
    if (hasBasicLayers) {
      if (!gridMap.isValid(*mapIterator)) {
        ++mapIterator;
        continue;
      }
    }

    Position3 position;
    if (!gridMap.getPosition3(pointLayer, *mapIterator, position)) {
      ++mapIterator;
      continue;
    }

    for (auto& iterator : fieldIterators) {
      if (iterator.first == "x") {
        *iterator.second = (float) position.x();
      } else if (iterator.first == "y") {
        *iterator.second = (float) position.y();
      } else if (iterator.first == "z") {
        *iterator.second = (float) position.z();
      } else if (iterator.first == "rgb") {
        *iterator.second = gridMap.at("color", *mapIterator);
      } else {
        *iterator.second = gridMap.at(iterator.first, *mapIterator);
      }
    }

    ++mapIterator;
    for (auto& iterator : fieldIterators) {
      ++iterator.second;
    }
    ++realNumberOfPoints;
  }

  if (realNumberOfPoints != maxNumberOfPoints) {
    pointCloud.width = realNumberOfPoints;
    pointCloud.row_step = pointCloud.width * pointCloud.point_step;
    pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
  }
}