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 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 point_field;
point_field.name = name;
point_field.count = 1;
point_field.datatype = sensor_msgs::PointField::FLOAT32;
point_field.offset = offset;
pointCloud.fields.push_back(point_field);
offset = offset + point_field.count * 4; // 4 for sensor_msgs::PointField::FLOAT32
}
// Resize.
size_t nPoints = gridMap.getSize().prod();
pointCloud.height = 1;
pointCloud.width = nPoints;
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);
for (size_t i = 0; i < nPoints; ++i) {
Position3 position;
position.setConstant(NAN);
gridMap.getPosition3(pointLayer, *mapIterator, position);
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;
}
}
}