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