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;
}
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);
}
}
