From 98c249b099a19e58e92c42a8c9b8f7bc485c9a0a Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 11:58:42 +0200 Subject: [PATCH 1/4] added assimp --- rviz_map_plugin/CMakeLists.txt | 8 + rviz_map_plugin/src/MapDisplay.cpp | 258 +++++++++++++++++------------ 2 files changed, 160 insertions(+), 106 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 3fc63da..90b90fa 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -17,6 +17,14 @@ find_package(Boost REQUIRED COMPONENTS system) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) find_package(OpenCL 2 REQUIRED) +find_package(assimp) + +if(assimp_FOUND) + include_directories(${ASSIMP_INCLUDE_DIRS}) + add_definitions(-DWITH_ASSIMP) +endif(assimp_FOUND) + + catkin_package( CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} DEPENDS Boost OpenCL HDF5 OpenCL diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index d6e40d6..93ba8cc 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -60,6 +60,12 @@ #include #include +#if defined(WITH_ASSIMP) + #include + #include + #include +#endif + namespace rviz_map_plugin { MapDisplay::MapDisplay() @@ -191,141 +197,181 @@ bool MapDisplay::loadData() setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file does not exist!"); return false; } - if (boost::filesystem::extension(mapFile).compare(".h5") != 0) - { - ROS_WARN_STREAM("Map Display: Specified map file is not a .h5 file!"); - setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file is not a .h5 file!"); - return false; - } + ROS_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); try { - // Open file IO - hdf5_map_io::HDF5MapIO map_io(mapFile); + if (boost::filesystem::extension(mapFile).compare(".h5") == 0) + { + ROS_INFO("Map Display: Load HDF5 map"); + // Open file IO + hdf5_map_io::HDF5MapIO map_io(mapFile); - ROS_INFO("Map Display: Load geometry"); + ROS_INFO("Map Display: Load geometry"); - // Read geometry - m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + // Read geometry + m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); - ROS_INFO("Map Display: Load textures"); + ROS_INFO("Map Display: Load textures"); - // Read textures - vector textures = map_io.getTextures(); - m_textures.resize(textures.size()); - for (size_t i = 0; i < textures.size(); i++) - { - // Find out the texture index because textures are not stored in ascending order - int textureIndex = std::stoi(textures[i].name); - - // Copy metadata - m_textures[textureIndex].width = textures[i].width; - m_textures[textureIndex].height = textures[i].height; - m_textures[textureIndex].channels = textures[i].channels; - m_textures[textureIndex].data = textures[i].data; - m_textures[textureIndex].pixelFormat = "rgb8"; - } + // Read textures + vector textures = map_io.getTextures(); + m_textures.resize(textures.size()); + for (size_t i = 0; i < textures.size(); i++) + { + // Find out the texture index because textures are not stored in ascending order + int textureIndex = std::stoi(textures[i].name); + + // Copy metadata + m_textures[textureIndex].width = textures[i].width; + m_textures[textureIndex].height = textures[i].height; + m_textures[textureIndex].channels = textures[i].channels; + m_textures[textureIndex].data = textures[i].data; + m_textures[textureIndex].pixelFormat = "rgb8"; + } - ROS_INFO("Map Display: Load materials"); + ROS_INFO("Map Display: Load materials"); - // Read materials - vector materials = map_io.getMaterials(); - vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); - m_materials.resize(materials.size()); - for (size_t i = 0; i < materials.size(); i++) - { - // Copy material color - m_materials[i].color.r = materials[i].r / 255.0f; - m_materials[i].color.g = materials[i].g / 255.0f; - m_materials[i].color.b = materials[i].b / 255.0f; - m_materials[i].color.a = 1.0f; - - // Look for texture index - if (materials[i].textureIndex == -1) + // Read materials + vector materials = map_io.getMaterials(); + vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); + m_materials.resize(materials.size()); + for (size_t i = 0; i < materials.size(); i++) { - // texture index -1: no texture - m_materials[i].textureIndex = boost::none; + // Copy material color + m_materials[i].color.r = materials[i].r / 255.0f; + m_materials[i].color.g = materials[i].g / 255.0f; + m_materials[i].color.b = materials[i].b / 255.0f; + m_materials[i].color.a = 1.0f; + + // Look for texture index + if (materials[i].textureIndex == -1) + { + // texture index -1: no texture + m_materials[i].textureIndex = boost::none; + } + else + { + m_materials[i].textureIndex = materials[i].textureIndex; + } + + m_materials[i].faceIndices.clear(); } - else + + // Copy face indices + for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) { - m_materials[i].textureIndex = materials[i].textureIndex; + m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); } - m_materials[i].faceIndices.clear(); - } - - // Copy face indices - for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) - { - m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); - } - - ROS_INFO("Map Display: Load vertex colors"); + ROS_INFO("Map Display: Load vertex colors"); - // Read vertex colors - vector colors = map_io.getVertexColors(); - m_colors.clear(); - m_colors.reserve(colors.size() / 3); - for (size_t i = 0; i < colors.size(); i += 3) - { - // convert from 0-255 (uint8) to 0.0-1.0 (float) - m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); - } + // Read vertex colors + vector colors = map_io.getVertexColors(); + m_colors.clear(); + m_colors.reserve(colors.size() / 3); + for (size_t i = 0; i < colors.size(); i += 3) + { + // convert from 0-255 (uint8) to 0.0-1.0 (float) + m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); + } - ROS_INFO("Map Display: Load vertex normals"); + ROS_INFO("Map Display: Load vertex normals"); - // Read vertex normals - vector normals = map_io.getVertexNormals(); - m_normals.clear(); - m_normals.reserve(normals.size() / 3); - for (size_t i = 0; i < normals.size(); i += 3) - { - m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); - } + // Read vertex normals + vector normals = map_io.getVertexNormals(); + m_normals.clear(); + m_normals.reserve(normals.size() / 3); + for (size_t i = 0; i < normals.size(); i += 3) + { + m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); + } - ROS_INFO("Map Display: Load texture coordinates"); + ROS_INFO("Map Display: Load texture coordinates"); - // Read tex cords - vector texCoords = map_io.getVertexTextureCoords(); - m_texCoords.clear(); - m_texCoords.reserve(texCoords.size() / 3); - for (size_t i = 0; i < texCoords.size(); i += 3) - { - m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); - } + // Read tex cords + vector texCoords = map_io.getVertexTextureCoords(); + m_texCoords.clear(); + m_texCoords.reserve(texCoords.size() / 3); + for (size_t i = 0; i < texCoords.size(); i += 3) + { + m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); + } - ROS_INFO("Map Display: Load clusters"); + ROS_INFO("Map Display: Load clusters"); - // Read labels - m_clusterList.clear(); - // m_clusterList.push_back(Cluster("__NEW__", vector())); - for (auto labelGroup : map_io.getLabelGroups()) - { - for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) + // Read labels + m_clusterList.clear(); + // m_clusterList.push_back(Cluster("__NEW__", vector())); + for (auto labelGroup : map_io.getLabelGroups()) { - auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); + for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) + { + auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); - std::stringstream ss; - ss << labelGroup << "_" << labelObj; - std::string label = ss.str(); + std::stringstream ss; + ss << labelGroup << "_" << labelObj; + std::string label = ss.str(); - m_clusterList.push_back(Cluster(label, faceIds)); + m_clusterList.push_back(Cluster(label, faceIds)); + } } - } - m_costs.clear(); - for (std::string costlayer : map_io.getCostLayers()) + m_costs.clear(); + for (std::string costlayer : map_io.getCostLayers()) + { + try + { + m_costs[costlayer] = map_io.getVertexCosts(costlayer); + } + catch (const hf::DataSpaceException& e) + { + ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); + } + } + } + #if defined(WITH_ASSIMP) + else { - try - { - m_costs[costlayer] = map_io.getVertexCosts(costlayer); - } - catch (const hf::DataSpaceException& e) - { - ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); - } + // PLY, OBJ, DAE? -> ASSIMP + // The following lines are a simple way to import the mesh geometry + // of commonly used mesh file formats. + // + // TODOs: + // 1. scene graphs will not be imported properly. + // Someone has to do some transformations according to the + // node graph in the assimp structures. Or optionally (even better): + // create tf-transformations for every element of the scene graph + // + Assimp::Importer io; + io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); + const aiScene* ascene = io.ReadFile(mapFile, 0); + const aiMesh* amesh = ascene->mMeshes[0]; + + const aiVector3D* ai_vertices = amesh->mVertices; + const aiFace* ai_faces = amesh->mFaces; + + m_geometry = std::make_shared(); + + m_geometry->vertices.resize(amesh->mNumVertices); + m_geometry->faces.resize(amesh->mNumFaces); + + for (int i = 0; i < amesh->mNumVertices; i++) + { + m_geometry->vertices[i].x = amesh->mVertices[i].x; + m_geometry->vertices[i].y = amesh->mVertices[i].y; + m_geometry->vertices[i].z = amesh->mVertices[i].z; + } + + for (int i = 0; i < amesh->mNumFaces; i++) + { + m_geometry->faces[i].vertexIndices[0] = amesh->mFaces[i].mIndices[0]; + m_geometry->faces[i].vertexIndices[1] = amesh->mFaces[i].mIndices[1]; + m_geometry->faces[i].vertexIndices[2] = amesh->mFaces[i].mIndices[2]; + } } + #endif // defined(WITH_ASSIMP) } catch (...) { From 1ff1a3e2a246f0c5ee8bc4f037248f0c4c0f4bb0 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 16:05:06 +0200 Subject: [PATCH 2/4] integrated assimp transforms for scaled, moved, or rotated meshes --- rviz_map_plugin/src/MapDisplay.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index 93ba8cc..4017cc7 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -346,7 +346,12 @@ bool MapDisplay::loadData() // Assimp::Importer io; io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); - const aiScene* ascene = io.ReadFile(mapFile, 0); + + // with aiProcess_PreTransformVertices assimp transforms the whole scene graph + // into one mesh + // - if you want to use TF for spawning meshes, the loading has to be done manually + const aiScene* ascene = io.ReadFile(mapFile, aiProcess_PreTransformVertices); + // what if there is more than one mesh? const aiMesh* amesh = ascene->mMeshes[0]; const aiVector3D* ai_vertices = amesh->mVertices; From a84506efaf999c39da42b7360d6ac1e2652c5a87 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 16:23:55 +0200 Subject: [PATCH 3/4] added file filters for common mesh file types once assimp is available --- rviz_map_plugin/src/RvizFileProperty.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rviz_map_plugin/src/RvizFileProperty.cpp b/rviz_map_plugin/src/RvizFileProperty.cpp index a9c32b1..94afdca 100644 --- a/rviz_map_plugin/src/RvizFileProperty.cpp +++ b/rviz_map_plugin/src/RvizFileProperty.cpp @@ -63,6 +63,16 @@ QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& QStringList filenameFilters; filenameFilters << tr("*.h5"); + #if defined(WITH_ASSIMP) + filenameFilters << tr("*.ply"); + filenameFilters << tr("*.obj"); + filenameFilters << tr("*.dae"); + filenameFilters << tr("*.stl"); + filenameFilters << tr("*.3d"); + filenameFilters << tr("*.3ds"); + filenameFilters << tr("*.fbx"); + filenameFilters << tr("*.blend"); + #endif filenameFilters << tr("*"); editor->setNameFilters(filenameFilters); From 1823ab894e0192354ac319deb25d77bfa30c4489 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 7 Sep 2023 13:53:09 +0200 Subject: [PATCH 4/4] added functionality to override rviz config with ros params --- rviz_map_plugin/include/MapDisplay.hpp | 8 ++++ rviz_map_plugin/src/MapDisplay.cpp | 64 ++++++++++++++++++++++++-- rviz_map_plugin/src/MeshDisplay.cpp | 2 +- 3 files changed, 70 insertions(+), 4 deletions(-) diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_map_plugin/include/MapDisplay.hpp index db9cc96..83dab27 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_map_plugin/include/MapDisplay.hpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -78,6 +79,7 @@ #include #include #include +#include #include #include @@ -149,6 +151,8 @@ class MapDisplay : public rviz::Display */ ~MapDisplay(); + virtual void load(const rviz::Config& config); + public Q_SLOTS: /** @@ -210,8 +214,12 @@ private Q_SLOTS: std::map> m_costs; + std::shared_ptr m_nh; + std::shared_ptr m_nh_p; + /// Path to map file rviz::FileProperty* m_mapFilePath; + std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) rviz_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index 4017cc7..bbd3f2d 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -107,8 +107,13 @@ rviz::Display* MapDisplay::createDisplay(const QString& class_id) void MapDisplay::onInitialize() { + std::string name = this->getName().toStdString(); + Display* display = createDisplay("rviz_map_plugin/ClusterLabel"); + m_nh = std::make_shared("~"); + m_nh_p = std::make_shared("~"); + m_clusterLabelDisplay = static_cast(display); m_clusterLabelDisplay->setName("ClusterLabel"); m_clusterLabelDisplay->setModel(model_); @@ -128,8 +133,6 @@ void MapDisplay::onInitialize() // Make signal/slot connections connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); - - updateMap(); } void MapDisplay::onEnable() @@ -147,14 +150,42 @@ void MapDisplay::onDisable() // ===================================================================================================================== // Callbacks triggered from UI events (mostly) +void MapDisplay::load(const rviz::Config& config) +{ + std::string name = this->getName().toStdString(); + std::cout << name << ": LOAD CONFIG..." << std::endl; + + rviz::Config config2 = config; + + { // Override with ros params + std::stringstream ss; + ss << "rviz_map_plugin/" << name; + + std::string mesh_file; + if(m_nh_p->getParam(ss.str(), mesh_file)) + { + config2.mapSetValue(m_mapFilePath->getName(), QString::fromStdString(mesh_file) ); + } else { + std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; + } + } + + rviz::Display::load(config2); + + std::cout << name << ": LOAD CONFIG done." << std::endl; +} + void MapDisplay::updateMap() { - ROS_INFO("Map Display: Update"); + std::string name = this->getName().toStdString(); + std::cout << name << ": updateMap" << std::endl; // Load geometry and clusters bool successful = loadData(); if (!successful) + { return; + } // Update sub-plugins m_meshDisplay->setGeometry(m_geometry); @@ -176,6 +207,8 @@ void MapDisplay::updateMap() // All good setStatus(rviz::StatusProperty::Ok, "Map", ""); + + m_map_file_loaded = m_mapFilePath->getFilename(); } // ===================================================================================================================== @@ -183,6 +216,30 @@ void MapDisplay::updateMap() bool MapDisplay::loadData() { + + + std::string name = this->getName().toStdString(); + // std::stringstream ss; + // ss << "rviz_map_plugin/" << name; + + // std::string mesh_file; + // if(m_nh_p->getParam(ss.str(), mesh_file)) + // { + // std::cout<< name << ": FOUND INITIAL MESH IN PARAMS - " << mesh_file << std::endl; + // m_mapFilePath->setFilename(QString::fromStdString(mesh_file)); + // } else { + // std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; + // } + + + if(m_mapFilePath->getFilename() == m_map_file_loaded) + { + std::cout << name << "! Tried to load same map twice. Skipping and keeping old data" << std::endl; + return true; + } + + + // Read map file path std::string mapFile = m_mapFilePath->getFilename(); if (mapFile.empty()) @@ -334,6 +391,7 @@ bool MapDisplay::loadData() #if defined(WITH_ASSIMP) else { + std::cout << "LOADING WITH ASSIMP" << std::endl; // PLY, OBJ, DAE? -> ASSIMP // The following lines are a simple way to import the mesh geometry // of commonly used mesh file formats. diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index f6ea39f..00872d0 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -77,7 +77,7 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized - m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Amount of meshes visualized", this, SLOT(updateBufferSize())); + m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); m_bufferSize->setMin(1); // Display Type