本文整理汇总了C++中grid_map::GridMap类的典型用法代码示例。如果您正苦于以下问题:C++ GridMap类的具体用法?C++ GridMap怎么用?C++ GridMap使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了GridMap类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: addColorLayerFromImage
bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image,
const std::string& layer,
grid_map::GridMap& gridMap)
{
cv_bridge::CvImagePtr cvPtr;
try {
cvPtr = cv_bridge::toCvCopy(image, image.encoding);
// cvPtr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); // FixMe
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return false;
}
gridMap.add(layer);
if (gridMap.getSize()(0) != image.height || gridMap.getSize()(1) != image.width) {
ROS_ERROR("Image size does not correspond to grid map size!");
return false;
}
for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
const auto& cvColor = cvPtr->image.at<cv::Vec3b>((*iterator)(0), (*iterator)(1));
Eigen::Vector3i colorVector;
// TODO Add cases for different image encodings.
colorVector(2) = cvColor[0]; // From BGR to RGB.
colorVector(1) = cvColor[1];
colorVector(0) = cvColor[2];
colorVectorToValue(colorVector, gridMap.at(layer, *iterator));
}
return true;
}
开发者ID:czalidis,项目名称:grid_map,代码行数:32,代码来源:GridMapRosConverter.cpp
示例2: ROS_ERROR
bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
cv::Mat& cvImage, const float dataMin, const float dataMax)
{
if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
// Initialize blank image.
cvImage = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), CV_8UC4);
} else {
ROS_ERROR("Invalid grid map?");
return false;
}
// Clamp outliers.
grid_map::GridMap map = gridMap;
map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(dataMin, dataMax));
// Find upper and lower values.
float lowerValue = map.get(layer).minCoeffOfFinites();
float upperValue = map.get(layer).maxCoeffOfFinites();
uchar imageMax = std::numeric_limits<unsigned char>::max();
for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
if (map.isValid(*iterator, layer)) {
float value = map.at(layer, *iterator);
uchar imageValue = (uchar)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax);
grid_map::Index imageIndex(iterator.getUnwrappedIndex());
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[0] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[1] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[2] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[3] = imageMax;
}
}
return true;
}
开发者ID:czalidis,项目名称:grid_map,代码行数:34,代码来源:GridMapRosConverter.cpp
示例3: initializeFromImage
bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image,
const double resolution, grid_map::GridMap& gridMap, const grid_map::Position& position)
{
const double lengthX = resolution * image.height;
const double lengthY = resolution * image.width;
Length length(lengthX, lengthY);
gridMap.setGeometry(length, resolution, position);
gridMap.setFrameId(image.header.frame_id);
gridMap.setTimestamp(image.header.stamp.toNSec());
return true;
}
开发者ID:czalidis,项目名称:grid_map,代码行数:11,代码来源:GridMapRosConverter.cpp
示例4: initialize
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;
}
开发者ID:ethz-asl,项目名称:grid_map,代码行数:12,代码来源:LineIterator.cpp
示例5: visualize
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_);
}
开发者ID:labimage,项目名称:grid_map,代码行数:57,代码来源:ElevationVisualization.cpp
示例6: getIndexLimitedToMapRange
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;
}
开发者ID:ethz-asl,项目名称:grid_map,代码行数:13,代码来源:LineIterator.cpp
示例7: setCostInPolygon
void ObjectsToCostmap::setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name,
const float score, grid_map::GridMap& objects_costmap)
{
grid_map::PolygonIterator iterators(objects_costmap, polygon);
for (grid_map::PolygonIterator iterator(objects_costmap, polygon); !iterator.isPastEnd(); ++iterator)
{
const float current_score = objects_costmap.at(gridmap_layer_name, *iterator);
if (score > current_score)
{
objects_costmap.at(gridmap_layer_name, *iterator) = score;
}
}
}
开发者ID:hatem-darweesh,项目名称:Autoware,代码行数:13,代码来源:objects_to_costmap.cpp
示例8: findSubmapParameters
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);
}
开发者ID:DHaylock,项目名称:grid_map,代码行数:14,代码来源:PolygonIterator.cpp
示例9: toMessage
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);
}
开发者ID:czalidis,项目名称:grid_map,代码行数:29,代码来源:GridMapRosConverter.cpp
示例10: generateRing
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();
}
开发者ID:DHaylock,项目名称:grid_map,代码行数:16,代码来源:SpiralIterator.cpp
示例11: computeElevationChange
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;
}
}
开发者ID:caomw,项目名称:elevation_mapping,代码行数:25,代码来源:ElevationChangeDetection.cpp
示例12: fromOccupancyGrid
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;
}
开发者ID:Semiustus,项目名称:csula_swarm_map,代码行数:44,代码来源:GridMapRosConverter.cpp
示例13: visualize
bool PointCloudVisualization::visualize(const grid_map::GridMap& map)
{
if (pointCloudPublisher_.getNumSubscribers () < 1) return true;
sensor_msgs::PointCloud2 pointCloud;
map.toPointCloud(pointCloud, pointType_);
pointCloudPublisher_.publish(pointCloud);
return true;
}
开发者ID:RIVeR-Lab,项目名称:ihmc-open-robotics-software,代码行数:11,代码来源:PointCloudVisualization.cpp
示例14: ROS_WARN_STREAM
bool GridCellsVisualization::visualize(const grid_map::GridMap& map)
{
if (!isActive()) return true;
if (!map.exists(layer_)) {
ROS_WARN_STREAM("GridCellsVisualization::visualize: No grid map layer with name '" << layer_ << "' found.");
return false;
}
nav_msgs::GridCells gridCells;
grid_map::GridMapRosConverter::toGridCells(map, layer_, lowerThreshold_, upperThreshold_, gridCells);
publisher_.publish(gridCells);
return true;
}
开发者ID:DHaylock,项目名称:grid_map,代码行数:12,代码来源:GridCellsVisualization.cpp
示例15: ROS_WARN_STREAM
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;
}
开发者ID:EricLYang,项目名称:grid_map,代码行数:49,代码来源:VectorVisualization.cpp
示例16: toOccupancyGrid
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;
}
}
开发者ID:Semiustus,项目名称:csula_swarm_map,代码行数:37,代码来源:GridMapRosConverter.cpp
示例17: FillPolygonAreas
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);
}
开发者ID:hatem-darweesh,项目名称:Autoware,代码行数:54,代码来源:object_map_utils.cpp
示例18: toOccupancyGrid
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;
}
}
开发者ID:czalidis,项目名称:grid_map,代码行数:38,代码来源:GridMapRosConverter.cpp
示例19: saveToBag
bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
const std::string& topic)
{
grid_map_msgs::GridMap message;
toMessage(gridMap, message);
ros::Time time(gridMap.getTimestamp());
if (!time.isValid() || time.isZero()) {
if (!ros::Time::isValid()) ros::Time::init();
time = ros::Time::now();
}
rosbag::Bag bag;
bag.open(pathToBag, rosbag::bagmode::Write);
bag.write(topic, time, message);
bag.close();
return true;
}
开发者ID:czalidis,项目名称:grid_map,代码行数:18,代码来源:GridMapRosConverter.cpp
示例20: toGridCells
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);
}
}
}
开发者ID:czalidis,项目名称:grid_map,代码行数:22,代码来源:GridMapRosConverter.cpp
注:本文中的grid_map::GridMap类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论