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