From e007855f1c1a90876dc552383b94ed68f5aa18de Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 5 Dec 2024 17:32:45 +0100 Subject: [PATCH 1/4] started to adapt to new lvr version --- hdf5_map_io/COLCON_IGNORE | 0 mesh_msgs_conversions/CMakeLists.txt | 9 +- .../mesh_msgs_conversions/conversions.h | 8 +- mesh_msgs_hdf5/COLCON_IGNORE | 0 mesh_tools/package.xml | 11 +- rviz_mesh_tools_plugins/CMakeLists.txt | 20 +- .../rviz_mesh_tools_plugins/MapDisplay.hpp | 2 - .../include/rviz_mesh_tools_plugins/Types.hpp | 2 + rviz_mesh_tools_plugins/package.xml | 2 +- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 274 +++++++++++------- 10 files changed, 187 insertions(+), 141 deletions(-) create mode 100644 hdf5_map_io/COLCON_IGNORE create mode 100644 mesh_msgs_hdf5/COLCON_IGNORE diff --git a/hdf5_map_io/COLCON_IGNORE b/hdf5_map_io/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_msgs_conversions/CMakeLists.txt b/mesh_msgs_conversions/CMakeLists.txt index 9e4a931..12be500 100644 --- a/mesh_msgs_conversions/CMakeLists.txt +++ b/mesh_msgs_conversions/CMakeLists.txt @@ -23,15 +23,10 @@ find_package(PkgConfig REQUIRED) add_definitions(${LVR2_DEFINITIONS} ${OpenCV_DEFINITIONS}) - -find_library(LVR2_LIBRARY NAMES lvr2) - # enable openmp support #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") - include_directories( include - ${catkin_INCLUDE_DIRS} ${LVR2_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) @@ -41,8 +36,8 @@ add_library(${PROJECT_NAME} ) target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$") + $ + $) target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} diff --git a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h index c594a59..4b6f9f0 100644 --- a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h +++ b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h @@ -35,13 +35,13 @@ #include "rclcpp/rclcpp.hpp" #include -#include -#include +#include +#include #include #include -#include -#include +#include +// #include #include #include diff --git a/mesh_msgs_hdf5/COLCON_IGNORE b/mesh_msgs_hdf5/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_tools/package.xml b/mesh_tools/package.xml index 0355e23..c46de10 100644 --- a/mesh_tools/package.xml +++ b/mesh_tools/package.xml @@ -14,13 +14,10 @@ ament_cmake - hdf5_map_io - - - - - - + mesh_msgs + mesh_msgs_transform + mesh_msgs_conversions + rviz_mesh_tools_plugins ament_cmake diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 0a313fb..bf0987b 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(rviz_rendering REQUIRED) find_package(rviz_ogre_vendor REQUIRED) find_package(rviz_assimp_vendor REQUIRED) find_package(std_msgs REQUIRED) -find_package(hdf5_map_io REQUIRED) find_package(mesh_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) @@ -39,7 +38,7 @@ find_package(HDF5 REQUIRED COMPONENTS C CXX HL) # LVR2 includes HighFive that we need here # - it was compiling before because globale lvr2 header were accessible -# find_package(LVR2 REQUIRED) +find_package(LVR2 REQUIRED) ## This setting causes Qt's "MOC" generation to happen automatically. # set(CMAKE_AUTOMOC ON) @@ -49,7 +48,7 @@ find_package(Qt5 COMPONENTS Core Widgets) include_directories( include - ${catkin_INCLUDE_DIRS} + ${LVR2_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS} ${OpenCL_INCLUDE_DIRS} @@ -87,18 +86,11 @@ set(MOC_HEADER_FILES include/rviz_mesh_tools_plugins/InteractionHelper.hpp ) -# foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") -# qt5_wrap_cpp(RVIZ_MESH_TOOLS_PLUGINS_MOC "${header}") -# endforeach() - - +add_definitions(${LVR2_DEFINITIONS}) add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} - ${MOC_HEADER_FILES} - ${HDF5_LIBRARIES} - ${HDF5_HL_LIBRARIES} - ${OpenCL_LIBRARIES} + ${MOC_HEADER_FILES} ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -108,6 +100,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PUBLIC + ${LVR2_LIBRARIES} rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay Boost::system @@ -132,7 +125,6 @@ ament_target_dependencies(${PROJECT_NAME} rviz_rendering rviz_assimp_vendor std_msgs - hdf5_map_io mesh_msgs resource_retriever tf2_ros @@ -153,11 +145,11 @@ ament_export_dependencies( rviz_rendering rviz_assimp_vendor std_msgs - hdf5_map_io mesh_msgs resource_retriever tf2_ros message_filters + std_msgs ) install( diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp index 9292c0c..c8b5b2f 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp @@ -91,8 +91,6 @@ #include #include -#include - #ifndef Q_MOC_RUN #include diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp index 3e0149b..2732475 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp @@ -143,6 +143,8 @@ struct Geometry faces.push_back(face); } } + + }; /// Struct for colors diff --git a/rviz_mesh_tools_plugins/package.xml b/rviz_mesh_tools_plugins/package.xml index 35ed6fb..8e091cc 100644 --- a/rviz_mesh_tools_plugins/package.xml +++ b/rviz_mesh_tools_plugins/package.xml @@ -23,12 +23,12 @@ rviz_ogre_vendor rclcpp + lvr2 rviz_common rviz_rendering rviz_assimp_vendor std_msgs mesh_msgs - hdf5_map_io tf2_ros pluginlib message_filters diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 8dfbeda..0b84c99 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -70,8 +70,14 @@ #include #include +#include + + +using HDF5MeshIO = lvr2::Hdf5Build; + namespace rviz_mesh_tools_plugins { + MapDisplay::MapDisplay() :m_clusterLabelDisplay(nullptr) ,m_meshDisplay(nullptr) @@ -364,131 +370,186 @@ bool MapDisplay::loadData() enableMeshDisplay(); RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load HDF5 map"); - // Open file IO - hdf5_map_io::HDF5MapIO map_io(mapFile); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load geometry"); + auto hdf5_mesh_io = std::make_shared(); - // Read geometry - m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + hdf5_mesh_io->open(mapFile); + // hdf5_mesh_io->setMeshName("mesh"); // TODO: dynamic - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load textures"); + auto mesh_buffer = hdf5_mesh_io->MeshIO::load("mesh"); - // 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"; - } + std::cout << *mesh_buffer << std::endl; - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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) - { - // texture index -1: no texture - m_materials[i].textureIndex = boost::none; - } - else + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load geometry"); + m_geometry = std::make_shared(); + + { // fill + lvr2::Channel lvr_vertices = *mesh_buffer->getChannel("vertices"); + + m_geometry->vertices.resize(lvr_vertices.numElements()); + for(size_t i=0; ivertices[i].x = lvr_vertices[i][0]; + m_geometry->vertices[i].y = lvr_vertices[i][1]; + m_geometry->vertices[i].z = lvr_vertices[i][2]; } - m_materials[i].faceIndices.clear(); - } + lvr2::Channel lvr_face_ids = *mesh_buffer->getChannel("face_indices"); - // Copy face indices - for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) - { - m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); - } - - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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)); + m_geometry->faces.resize(lvr_face_ids.numElements()); + for(size_t i=0; ifaces[i].vertexIndices[0] = lvr_face_ids[i][0]; + m_geometry->faces[i].vertexIndices[1] = lvr_face_ids[i][1]; + m_geometry->faces[i].vertexIndices[2] = lvr_face_ids[i][2]; + } } - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex normals"); - // Read vertex normals - vector normals = map_io.getVertexNormals(); + // vector normals = map_io.getVertexNormals(); m_normals.clear(); - m_normals.reserve(normals.size() / 3); - for (size_t i = 0; i < normals.size(); i += 3) + auto lvr2_vertex_normals_opt = mesh_buffer->getChannel("vertex_normals"); + if(lvr2_vertex_normals_opt) { - m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); + lvr2::Channel lvr2_vertex_normals = *lvr2_vertex_normals_opt; + m_normals.reserve(lvr2_vertex_normals.numElements()); + for (size_t i = 0; i < lvr2_vertex_normals.numElements(); i++) + { + m_normals.push_back(Normal(lvr2_vertex_normals[i][0], lvr2_vertex_normals[i][1], lvr2_vertex_normals[i][2])); + } } - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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])); - } - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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)) - { - auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); + /// OLD + + // // Open file IO + // hdf5_map_io::HDF5MapIO map_io(mapFile); - std::stringstream ss; - ss << labelGroup << "_" << labelObj; - std::string label = ss.str(); + // Read geometry - m_clusterList.push_back(Cluster(label, faceIds)); - } - } + // m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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"; + // } + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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) + // { + // // texture index -1: no texture + // m_materials[i].textureIndex = boost::none; + // } + // else + // { + // m_materials[i].textureIndex = materials[i].textureIndex; + // } + + // 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); + // } + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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)); + // } + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex normals"); - m_costs.clear(); - for (std::string costlayer : map_io.getCostLayers()) - { - try - { - m_costs[costlayer] = map_io.getVertexCosts(costlayer); - } - catch (const hf::DataSpaceException& e) - { - RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); - } - } + // 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])); + // } + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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])); + // } + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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)) + // { + // auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); + + // std::stringstream ss; + // ss << labelGroup << "_" << labelObj; + // std::string label = ss.str(); + + // m_clusterList.push_back(Cluster(label, faceIds)); + // } + // } + + // m_costs.clear(); + // for (std::string costlayer : map_io.getCostLayers()) + // { + // try + // { + // m_costs[costlayer] = map_io.getVertexCosts(costlayer); + // } + // catch (const hf::DataSpaceException& e) + // { + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); + // } + // } } else { @@ -731,11 +792,12 @@ void MapDisplay::saveLabel(Cluster cluster) return; } - // Open IO - hdf5_map_io::HDF5MapIO map_io(m_mapFilePath->getFilename()); + // is not working anyway + // // Open IO + // hdf5_map_io::HDF5MapIO map_io(m_mapFilePath->getFilename()); - // Add label with faces list - map_io.addOrUpdateLabel(results[0], results[1], faces); + // // Add label with faces list + // map_io.addOrUpdateLabel(results[0], results[1], faces); // Add to cluster list m_clusterList.push_back(Cluster(label, faces)); From b9ceb390ba59b01d1ab17051e75be833c20b4e0d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 6 Dec 2024 15:22:14 +0100 Subject: [PATCH 2/4] compiles with new version of lvr now --- hdf5_map_io/CHANGELOG.rst | 24 - hdf5_map_io/CMakeLists.txt | 72 --- hdf5_map_io/COLCON_IGNORE | 0 hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h | 311 ------------ hdf5_map_io/package.xml | 25 - hdf5_map_io/src/hdf5_map_io.cpp | 466 ------------------ .../mesh_msgs_conversions/conversions.h | 63 ++- .../include/rviz_mesh_tools_plugins/Types.hpp | 2 - rviz_mesh_tools_plugins/src/MapDisplay.cpp | 160 +++--- 9 files changed, 161 insertions(+), 962 deletions(-) delete mode 100644 hdf5_map_io/CHANGELOG.rst delete mode 100644 hdf5_map_io/CMakeLists.txt delete mode 100644 hdf5_map_io/COLCON_IGNORE delete mode 100644 hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h delete mode 100644 hdf5_map_io/package.xml delete mode 100644 hdf5_map_io/src/hdf5_map_io.cpp diff --git a/hdf5_map_io/CHANGELOG.rst b/hdf5_map_io/CHANGELOG.rst deleted file mode 100644 index 08c1bbe..0000000 --- a/hdf5_map_io/CHANGELOG.rst +++ /dev/null @@ -1,24 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package hdf5_map_io -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (2024-05-15) ------------------- -* port package to ROS2 - -1.1.0 (2021-10-27) ------------------- -* added publishing of mesh_msgs to mesh_msgs_hdf5 package -* fixed mpi error -* resolved catkin lint problems -* update label in hdf5 file - -1.0.1 (2020-11-11) ------------------- -* fix: find_library lvr2, since this is missing in LVR2Config.cmake -* use new hdf5 structure compatible to the datasets in https://github.com/uos/pluto_robot -* use HighFive from lvr2, add lvr2 as dependency and removed HighFive gitmodule definition - -1.0.0 (2020-04-26) ------------------- -* release version 1.0.0 \ No newline at end of file diff --git a/hdf5_map_io/CMakeLists.txt b/hdf5_map_io/CMakeLists.txt deleted file mode 100644 index 4d348a2..0000000 --- a/hdf5_map_io/CMakeLists.txt +++ /dev/null @@ -1,72 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(hdf5_map_io) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# DEFAULT RELEASE -if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) - if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) - endif() -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(LVR2 REQUIRED) -find_package(MPI) -find_package(PkgConfig REQUIRED) - - -# HighFive -set(HIGHFIVE_EXAMPLES FALSE) -set(HIGHFIVE_UNIT_TESTS FALSE) - -find_package(LVR2 REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/hdf5_map_io.cpp -) - -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" - ${LVR2_INCLUDE_DIRS}) - -target_link_libraries(${PROJECT_NAME} - ${LVR2_LIBRARIES} - ${MPI_CXX_LIBRARIES} - rclcpp::rclcpp -) - -target_compile_definitions(${PROJECT_NAME} PRIVATE "HDF5_MAP_IO_BUILDING_DLL") - -install(TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include -) - -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_libraries(${PROJECT_NAME}) - -# Export modern CMake targets -ament_export_targets(export_${PROJECT_NAME}) - -ament_export_dependencies( - rclcpp - LVR2 -) - -ament_package() \ No newline at end of file diff --git a/hdf5_map_io/COLCON_IGNORE b/hdf5_map_io/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h b/hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h deleted file mode 100644 index c5ba8da..0000000 --- a/hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h +++ /dev/null @@ -1,311 +0,0 @@ -#ifndef HDF5_MAP_IO__H_ -#define HDF5_MAP_IO__H_ - -#include -#include -#include -#include - -#include -#include - -namespace hf = HighFive; - -namespace hdf5_map_io -{ - -/** - * Helper struct to save vertices to the map. - */ -struct MapVertex { - float x; - float y; - float z; -}; - - -/** - * Helper struct to save textures / images to the map. - */ -struct MapImage { - std::string name; - uint32_t width; - uint32_t height; - uint32_t channels; - std::vector data; -}; - -/** - * Helper struct for saving material data to the map. - * - * This struct is defined as an HDF compound data type. - */ -struct MapMaterial { - int32_t textureIndex; - uint8_t r; - uint8_t g; - uint8_t b; -}; - -/** - * This class if responsible for the map format. It tries to abstract most if not all calls to the - * underlying HDF5 API and the HighFive wrapper. Furthermore it ensures the defined map format is always - * in place and not tinkered with. - * - * NOTE: the map file is held open for the whole live time of this object. Thus it is possible that some data is - * only written to disc if the destructor is called or the program has ended. Also make sure the map file is not opened - * in any other way. (i.e. with the HDF5 Viewer). This will always lead to errors trying to access the file. - */ -class HDF5MapIO -{ -public: - /** - * @brief Opens a map file for reading and writing. - */ - HDF5MapIO(std::string filename); - - /** - * @brief Creates a map file (or truncates if the file already exists). - */ - HDF5MapIO( - std::string filename, - const std::vector& vertices, - const std::vector& face_ids - ); - - /** - * @brief Closes main groups and makes sure all buffers are flushed to the file on disc. - */ - ~HDF5MapIO(); - - /** - * @brief Returns vertices vector - */ - std::vector getVertices(); - - /** - * @brief Returns face ids vector - */ - std::vector getFaceIds(); - - /** - * @brief Returns vertex normals vector - */ - std::vector getVertexNormals(); - - /** - * @brief Returns vertex colors vector - */ - std::vector getVertexColors(); - - /** - * @brief Returns textures vector - */ - std::vector getTextures(); - - /** - * @brief Returns an map which keys are representing the features point in space and the values - * are an vector of floats representing the keypoints. - */ - std::unordered_map> getFeatures(); - - /** - * @brief Returns materials as MapMaterial - */ - std::vector getMaterials(); - - /** - * @brief Returns material <-> face indices - */ - std::vector getMaterialFaceIndices(); - - /** - * @brief Returns vertex texture coordinates - */ - std::vector getVertexTextureCoords(); - - /** - * @brief Returns all available label groups - */ - std::vector getLabelGroups(); - - /** - * @brief Returns all labels inside the given group - */ - std::vector getAllLabelsOfGroup(std::string groupName); - - /** - * @brief Returns face ids for the given label inside the group. - * E.g: label=tree_1 -> groupName=tree; labelName=1 - */ - std::vector getFaceIdsOfLabel(std::string groupName, std::string labelName); - - /** - * @brief Returns the roughness as float vector. - */ - std::vector getRoughness(); - - /** - * @brief Returns the height difference as float vector. - */ - std::vector getHeightDifference(); - - /** - * @brief Returns one costlayer as float vector. - */ - std::vector getVertexCosts(std::string costlayer); - - /** - * @brief returns the names of all available costlayers - */ - std::vector getCostLayers(); - - /** - * @brief Returns the image in the group, if it exists. If not an empty struct is returned - */ - MapImage getImage(hf::Group group, std::string name); - - /** - * @brief Add normals to the attributes group. - */ - hf::DataSet addVertexNormals(std::vector& normals); - - /** - * @brief Add vertex colors to the attributes group. - */ - hf::DataSet addVertexColors(std::vector& colors); - - /** - * Add texture img with given index to the textures group. Texture CAN NOT be overridden - */ - void addTexture(int index, uint32_t width, uint32_t height, uint8_t* data); - - /** - * @brief Add materials as MapMaterial and the corresponding material <-> face indices - */ - void addMaterials(std::vector& materials, std::vector& matFaceIndices); - - /** - * @brief Add vertex texture coordinates to the textures group. - */ - void addVertexTextureCoords(std::vector& coords); - - /** - * @brief Adds the label (labelName) to the label group with the given faces. - * E.g.: tree_1 -> groupName=tree; labelName=1; separated by the '_' - */ - void addLabel(std::string groupName, std::string labelName, std::vector& faceIds); - - /** - * @brief Adds or updates the label (labelName) to the label group with the given faces. - * E.g.: tree_1 -> groupName=tree; labelName=1; separated by the '_' - */ - void addOrUpdateLabel(std::string groupName, std::string labelName, std::vector& faceIds); - - /** - * @brief Adds the keypoints with their corresponding positions to the attributes_group. The position - * is saved to the entry via an attribute called 'vector'. - */ - void addTextureKeypointsMap(std::unordered_map>& keypoints_map); - - /** - * @brief Adds the roughness to the attributes group. - */ - void addRoughness(std::vector& roughness); - - /** - * @brief Adds the height difference to the attributes group. - */ - void addHeightDifference(std::vector& diff); - - /** - * @brief Adds an image with given data set name to the given group - */ - void addImage(hf::Group group, - std::string name, - const uint32_t width, - const uint32_t height, - const uint8_t* pixelBuffer - ); - - /** - * Removes all labels from the file. - *
- * Be careful, this does not clear up the space of the labels. Use the cli tool 'h5repack' manually to clear up - * all wasted space if this method was used multiple times. - * - * @return true if removing all labels successfully. - */ - bool removeAllLabels(); - - /** - * @brief Flushes the file. All opened buffers are saved to disc. - */ - void flush(); - -private: - hf::File m_file; - - void creatOrGetGroups(); - - size_t getSize(hf::DataSet& data_set); - // group names - static constexpr const char* CHANNELS_GROUP = "/mesh/channels"; - static constexpr const char* CLUSTERSETS_GROUP = "/mesh/clustersets"; - static constexpr const char* TEXTURES_GROUP = "/mesh/textures"; - static constexpr const char* LABELS_GROUP = "/mesh/labels"; - - // main groups for reference - hf::Group m_channelsGroup; - hf::Group m_clusterSetsGroup; - hf::Group m_texturesGroup; - hf::Group m_labelsGroup; -}; -} // namespace hdf5_map_io - - -namespace HighFive { - -/** - * Define the MapMaterial as an HDF5 compound data type. - */ -template <> -inline AtomicType::AtomicType() -{ - hid_t materialHid = H5Tcreate(H5T_COMPOUND, sizeof(hdf5_map_io::MapMaterial)); - - H5Tinsert(materialHid, "textureIndex", offsetof(hdf5_map_io::MapMaterial, textureIndex), H5T_NATIVE_INT); - H5Tinsert(materialHid, "r", offsetof(hdf5_map_io::MapMaterial, r), H5T_NATIVE_UCHAR); - H5Tinsert(materialHid, "g", offsetof(hdf5_map_io::MapMaterial, g), H5T_NATIVE_UCHAR); - H5Tinsert(materialHid, "b", offsetof(hdf5_map_io::MapMaterial, b), H5T_NATIVE_UCHAR); - - _hid = H5Tcopy(materialHid); -} - -} - -namespace std -{ -template <> -struct hash -{ - size_t operator()(const hdf5_map_io::MapVertex& k) const - { - size_t h1 = std::hash()(k.x); - size_t h2 = std::hash()(k.y); - size_t h3 = std::hash()(k.z); - return (h1 ^ (h2 << 1)) ^ h3; - } -}; - -template <> -struct equal_to -{ - bool operator()( const hdf5_map_io::MapVertex& lhs, const hdf5_map_io::MapVertex& rhs ) const{ - return (lhs.x == rhs.x) && (lhs.y == rhs.y) && (lhs.z == rhs.z); - } -}; - -} - -#endif // HDF5_MAP_IO__H_ diff --git a/hdf5_map_io/package.xml b/hdf5_map_io/package.xml deleted file mode 100644 index 8ffe215..0000000 --- a/hdf5_map_io/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - hdf5_map_io - 2.0.0 - The hdf5_map_io package - Alexander Mock - Matthias Holoch - Sebastian Pütz - BSD-3 - http://wiki.ros.org/ros_mesh_tools/hdf5_map_io - Sebastian Pütz - - ament_cmake - rclcpp - boost - lvr2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/hdf5_map_io/src/hdf5_map_io.cpp b/hdf5_map_io/src/hdf5_map_io.cpp deleted file mode 100644 index 8c6511d..0000000 --- a/hdf5_map_io/src/hdf5_map_io.cpp +++ /dev/null @@ -1,466 +0,0 @@ -#include "hdf5_map_io/hdf5_map_io.h" -#include -#include - -namespace hdf5_map_io -{ - -void HDF5MapIO::creatOrGetGroups() -{ - if (!m_file.exist(CHANNELS_GROUP)) - m_channelsGroup = m_file.createGroup(CHANNELS_GROUP); - else - m_channelsGroup = m_file.getGroup(CHANNELS_GROUP); - - if(!m_file.exist(CLUSTERSETS_GROUP)) - m_clusterSetsGroup = m_file.createGroup(CLUSTERSETS_GROUP); - else - m_clusterSetsGroup = m_file.getGroup(CLUSTERSETS_GROUP); - - if(!m_file.exist(TEXTURES_GROUP)) - m_texturesGroup = m_file.createGroup(TEXTURES_GROUP); - else - m_texturesGroup = m_file.getGroup(TEXTURES_GROUP); - - if(!m_file.exist(LABELS_GROUP)) - m_labelsGroup = m_file.createGroup(LABELS_GROUP); - else - m_labelsGroup = m_file.getGroup(LABELS_GROUP); -} - -HDF5MapIO::HDF5MapIO(std::string filename) - : m_file(filename, hf::File::ReadWrite) -{ - creatOrGetGroups(); -} - -HDF5MapIO::HDF5MapIO( - std::string filename, - const std::vector& vertices, - const std::vector& face_ids -) - : m_file(filename, hf::File::ReadWrite | hf::File::Create | hf::File::Truncate) -{ - - if (!m_file.isValid()) - throw "Could not open file."; - - creatOrGetGroups(); - - // Create geometry data sets - m_channelsGroup - .createDataSet("vertices", hf::DataSpace::From(vertices)) - .write(vertices); - m_channelsGroup - .createDataSet("face_indices", hf::DataSpace::From(face_ids)) - .write(face_ids); -} - -HDF5MapIO::~HDF5MapIO() -{ - if (!m_file.isValid()) - { - // do nothing if file is not valid, i.e. already closed - return; - } - - H5Gclose(m_channelsGroup.getId()); - H5Gclose(m_clusterSetsGroup.getId()); - H5Gclose(m_texturesGroup.getId()); - H5Gclose(m_labelsGroup.getId()); - H5Fclose(m_file.getId()); -} - -size_t HDF5MapIO::getSize(hf::DataSet& data_set) -{ - auto dimensions = data_set.getSpace().getDimensions(); - return dimensions[0] * dimensions[1]; -} - -std::vector HDF5MapIO::getVertices() -{ - std::vector vertices; - if (!m_channelsGroup.exist("vertices")) - return vertices; - auto dataset = m_channelsGroup.getDataSet("vertices"); - vertices.resize(getSize(dataset)); - dataset.read(vertices.data()); - return vertices; -} - -std::vector HDF5MapIO::getFaceIds() -{ - std::vector indices; - if (!m_channelsGroup.exist("face_indices")) - return indices; - auto dataset = m_channelsGroup.getDataSet("face_indices"); - indices.resize(getSize(dataset)); - dataset.read(indices.data()); - return indices; -} - -std::vector HDF5MapIO::getVertexNormals() -{ - std::vector normals; - if (!m_channelsGroup.exist("vertex_normals")) - return normals; - auto dataset = m_channelsGroup.getDataSet("vertex_normals"); - normals.resize(getSize(dataset)); - dataset.read(normals.data()); - return normals; -} - -std::vector HDF5MapIO::getVertexColors() -{ - std::vector colors; - if (!m_channelsGroup.exist("vertex_colors")) - return colors; - - auto dataset = m_channelsGroup.getDataSet("vertex_colors"); - colors.resize(getSize(dataset)); - dataset.read(colors.data()); - return colors; -} - -std::vector HDF5MapIO::getTextures() -{ - std::vector textures; - if (!m_texturesGroup.exist("images")) - { - return textures; - } - - const hf::Group& imagesGroup = m_texturesGroup.getGroup("images"); - for (auto setName: imagesGroup.listObjectNames()) - { - textures.push_back(getImage(imagesGroup, setName)); - } - - return textures; -} - -std::unordered_map> HDF5MapIO::getFeatures() -{ - std::unordered_map> features; - - if (!m_channelsGroup.exist("texture_features")) - { - return features; - } - - const auto& featuresGroup = m_channelsGroup.getGroup("texture_features"); - features.reserve(featuresGroup.getNumberObjects()); - - for (auto name : featuresGroup.listObjectNames()) - { - // fill vector with descriptor - std::vector descriptor; - auto dataset = featuresGroup.getDataSet(name); - dataset.read(descriptor); - - // read vector attribute with xyz coords - MapVertex v; - std::vector xyz(3); - auto vector_attr = dataset.getAttribute("vector"); - vector_attr.read(xyz); - - v.x = xyz[0]; - v.y = xyz[1]; - v.z = xyz[2]; - - features.insert({v, descriptor}); - } - - return features; -} - -std::vector HDF5MapIO::getMaterials() -{ - std::vector materials; - - if (!m_texturesGroup.exist("materials")) - { - return materials; - } - - m_texturesGroup.getDataSet("materials") - .read(materials); - - return materials; -} - -std::vector HDF5MapIO::getMaterialFaceIndices() -{ - std::vector matFaceIndices; - - if (!m_texturesGroup.exist("mat_face_indices")) - { - return matFaceIndices; - } - - m_texturesGroup.getDataSet("mat_face_indices") - .read(matFaceIndices); - - return matFaceIndices; -} - -std::vector HDF5MapIO::getVertexTextureCoords() -{ - std::vector coords; - - if (!m_texturesGroup.exist("coords")) - { - return coords; - } - - m_texturesGroup.getDataSet("coords") - .read(coords); - - return coords; -} - -std::vector HDF5MapIO::getLabelGroups() -{ - return m_labelsGroup.listObjectNames(); -} - -std::vector HDF5MapIO::getAllLabelsOfGroup(std::string groupName) -{ - if (!m_labelsGroup.exist(groupName)) - { - return std::vector(); - } - - return m_labelsGroup.getGroup(groupName).listObjectNames(); -} - -std::vector HDF5MapIO::getFaceIdsOfLabel(std::string groupName, std::string labelName) -{ - std::vector faceIds; - - if (!m_labelsGroup.exist(groupName)) - { - return faceIds; - } - - auto lg = m_labelsGroup.getGroup(groupName); - - if (!lg.exist(labelName)) - { - return faceIds; - } - - lg.getDataSet(labelName).read(faceIds); - - return faceIds; -} - -std::vector HDF5MapIO::getRoughness() -{ - return getVertexCosts("roughness"); -} - -std::vector HDF5MapIO::getHeightDifference() -{ - return getVertexCosts("height_diff"); -} - -std::vector HDF5MapIO::getVertexCosts(std::string costlayer) -{ - std::vector costs; - - if (!m_channelsGroup.exist(costlayer)) - { - return costs; - } - - m_channelsGroup.getDataSet(costlayer) - .read(costs); - - return costs; -} - -std::vector HDF5MapIO::getCostLayers() -{ - return m_channelsGroup.listObjectNames(); -} - -MapImage HDF5MapIO::getImage(hf::Group group, std::string name) -{ - MapImage t; - - if (!group.exist(name)) - { - return t; - } - - hsize_t width; - hsize_t height; - hsize_t pixel_size; - char interlace[20]; - hssize_t npals; - - H5IMget_image_info(group.getId(), name.c_str(), &width, &height, &pixel_size, interlace, &npals); - - auto bufSize = width * height * pixel_size; - std::vector buf; - buf.resize(bufSize); - H5IMread_image(group.getId(), name.c_str(), buf.data()); - - t.name = name; - t.width = width; - t.height = height; - t.channels = pixel_size; - t.data = buf; - - return t; -} - -hf::DataSet HDF5MapIO::addVertexNormals(std::vector& normals) -{ - // TODO make more versatile to add and/or overwrite normals in file - auto dataSet = m_channelsGroup.createDataSet("vertex_normals", hf::DataSpace::From(normals)); - dataSet.write(normals); - - return dataSet; -} - -hf::DataSet HDF5MapIO::addVertexColors(std::vector& colors) -{ - auto dataSet = m_channelsGroup.createDataSet("vertex_colors", hf::DataSpace::From(colors)); - dataSet.write(colors); - - return dataSet; -} - -void HDF5MapIO::addTexture(int index, uint32_t width, uint32_t height, uint8_t *data) -{ - if (!m_texturesGroup.exist("images")) - { - m_texturesGroup.createGroup("images"); - } - - auto imagesGroup = m_texturesGroup.getGroup("images"); - const std::string& name = std::to_string(index); - - if (imagesGroup.exist(name)) - { - return; - } - - addImage(imagesGroup, name, width, height, data); -} - -void HDF5MapIO::addMaterials(std::vector& materials, std::vector& matFaceIndices) -{ - m_texturesGroup - .createDataSet("materials", hf::DataSpace::From(materials)) - .write(materials); - - m_texturesGroup - .createDataSet("mat_face_indices", hf::DataSpace::From(matFaceIndices)) - .write(matFaceIndices); -} - -void HDF5MapIO::addVertexTextureCoords(std::vector& coords) -{ - m_texturesGroup - .createDataSet("coords", hf::DataSpace::From(coords)) - .write(coords); -} - -void HDF5MapIO::addOrUpdateLabel(std::string groupName, std::string labelName, std::vector& faceIds) -{ - std::cout << "Add or update label" << std::endl; - if (!m_labelsGroup.exist(groupName)) - { - m_labelsGroup.createGroup(groupName); - } - - auto group = m_labelsGroup.getGroup(groupName); - if(group.exist(labelName)) - { - std::cout << "write to existing label" << std::endl; - auto dataset = group.getDataSet(labelName); - dataset.write(faceIds); - } - else - { - std::cout << "write to new label" << std::endl; - auto dataset = group.createDataSet(labelName, hf::DataSpace::From(faceIds)); - dataset.write(faceIds); - } -} - -void HDF5MapIO::addLabel(std::string groupName, std::string labelName, std::vector& faceIds) -{ - if (!m_labelsGroup.exist(groupName)) - { - m_labelsGroup.createGroup(groupName); - } - - m_labelsGroup.getGroup(groupName) - .createDataSet(labelName, hf::DataSpace::From(faceIds)) - .write(faceIds); -} - -void HDF5MapIO::addTextureKeypointsMap(std::unordered_map>& keypoints_map) -{ - if (!m_channelsGroup.exist("texture_features")) - { - m_channelsGroup.createGroup("texture_features"); - } - - auto tf = m_channelsGroup.getGroup("texture_features"); - - size_t i = 0; - for (const auto& keypoint_features : keypoints_map) - { - auto dataset = tf.createDataSet(std::to_string(i), hf::DataSpace::From(keypoint_features.second)); - dataset.write(keypoint_features.second); - - std::vector v = {keypoint_features.first.x, keypoint_features.first.y, keypoint_features.first.z}; - dataset.template createAttribute("vector", hf::DataSpace::From(v)) - .write(v); - - i++; - } -} - -void HDF5MapIO::addRoughness(std::vector& roughness) -{ - m_channelsGroup.createDataSet("roughness", hf::DataSpace::From(roughness)) - .write(roughness); -} - -void HDF5MapIO::addHeightDifference(std::vector& diff) -{ - m_channelsGroup.createDataSet("height_diff", hf::DataSpace::From(diff)) - .write(diff); -} - -void HDF5MapIO::addImage(hf::Group group, std::string name, const uint32_t width, const uint32_t height, - const uint8_t *pixelBuffer) -{ - H5IMmake_image_24bit(group.getId(), name.c_str(), width, height, "INTERLACE_PIXEL", pixelBuffer); -} - -bool HDF5MapIO::removeAllLabels() -{ - bool result = true; - for (std::string name : m_labelsGroup.listObjectNames()) - { - std::string fullPath = std::string(LABELS_GROUP) + "/" + name; - result = H5Ldelete(m_file.getId(), fullPath.data(), H5P_DEFAULT) > 0; - } - - return result; -} - -void HDF5MapIO::flush() -{ - m_file.flush(); -} - -} // namespace hdf5_map_io - diff --git a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h index 4b6f9f0..d677c14 100644 --- a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h +++ b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h @@ -41,11 +41,11 @@ #include #include -// #include #include #include #include +#include #include #include "std_msgs/msg/string.h" @@ -88,6 +88,67 @@ struct MaterialGroup typedef std::vector > GroupVector; typedef boost::shared_ptr MaterialGroupPtr; +template +inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( + const std::shared_ptr>> hem, + const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>()) +{ + mesh_msgs::msg::MeshGeometry mesh_msg; + mesh_msg.vertices.reserve(hem->numVertices()); + mesh_msg.faces.reserve(hem->numFaces()); + + mesh_msg.vertex_normals.reserve(normals.numValues()); + + lvr2::DenseVertexMap new_indices; + new_indices.reserve(hem->numVertices()); + + size_t k = 0; + for(auto vH : hem->vertices()) + { + new_indices.insert(vH, k++); + const auto& pi = hem->getVertexPosition(vH); + geometry_msgs::msg::Point p; + p.x = pi.x; p.y = pi.y; p.z = pi.z; + mesh_msg.vertices.push_back(p); + } + + for(auto fH : hem->faces()) + { + mesh_msgs::msg::MeshTriangleIndices indices; + auto vHs = hem->getVerticesOfFace(fH); + indices.vertex_indices[0] = new_indices[vHs[0]]; + indices.vertex_indices[1] = new_indices[vHs[1]]; + indices.vertex_indices[2] = new_indices[vHs[2]]; + mesh_msg.faces.push_back(indices); + } + + for(auto vH : hem->vertices()) + { + const auto& n = normals[vH]; + geometry_msgs::msg::Point v; + v.x = n.x; v.y = n.y; v.z = n.z; + mesh_msg.vertex_normals.push_back(v); + } + + return mesh_msg; +} + +template +inline const mesh_msgs::msg::MeshGeometryStamped toMeshGeometryStamped( + const std::shared_ptr>> hem, + const std::string& frame_id, + const std::string& uuid, + const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>(), + const rclcpp::Time& stamp = rclcpp::Time()) +{ + mesh_msgs::msg::MeshGeometryStamped mesh_msg; + mesh_msg.mesh_geometry = toMeshGeometry(hem, normals); + mesh_msg.uuid = uuid; + mesh_msg.header.frame_id = frame_id; + mesh_msg.header.stamp = stamp; + return mesh_msg; +} + template inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp index 2732475..3e0149b 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp @@ -143,8 +143,6 @@ struct Geometry faces.push_back(face); } } - - }; /// Struct for colors diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 0b84c99..b66a080 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -72,12 +72,29 @@ #include +#include + using HDF5MeshIO = lvr2::Hdf5Build; namespace rviz_mesh_tools_plugins { +template +std::vector copy_1d_channel_to_vector(const lvr2::Channel& lvr_channel) +{ + if(lvr_channel.width() != 1) + { + throw std::runtime_error("channel is not 1d!"); + } + std::vector ret(lvr_channel.numElements()); + for(size_t i=0; i(); hdf5_mesh_io->open(mapFile); - // hdf5_mesh_io->setMeshName("mesh"); // TODO: dynamic - - auto mesh_buffer = hdf5_mesh_io->MeshIO::load("mesh"); + auto mesh_buffer = hdf5_mesh_io->MeshIO::load("mesh"); // TODO dynamic + // the mesh buffer is a map from a string to a Channel + // example: + // "vertices" -> Channel std::cout << *mesh_buffer << std::endl; - + // "mesh_navigatio_tutorials/maps/floor_is_lava.h5" + // [ VariantChannelMap ] + // vertex_normals: type: float, size: [8100,3] + // vertices: type: float, size: [8100,3] + // roughness: type: float, size: [8100,1] + // height_diff: type: float, size: [8100,1] + // freespace: type: float, size: [8100,1] + // face_indices: type: unsigned int, size: [15960,3] + // edge_distances_idx: type: unsigned int, size: [24059,1] + // edge_distances: type: float, size: [24059,1] + // border: type: float, size: [8100,1] + // face_normals: type: float, size: [15960,3] + // average_angles: type: float, size: [8100,1] RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load geometry"); - m_geometry = std::make_shared(); + // Read geometry + m_geometry = std::make_shared(); { // fill lvr2::Channel lvr_vertices = *mesh_buffer->getChannel("vertices"); @@ -407,35 +438,13 @@ bool MapDisplay::loadData() } } - // Read vertex normals - // vector normals = map_io.getVertexNormals(); - m_normals.clear(); - auto lvr2_vertex_normals_opt = mesh_buffer->getChannel("vertex_normals"); - if(lvr2_vertex_normals_opt) - { - lvr2::Channel lvr2_vertex_normals = *lvr2_vertex_normals_opt; - m_normals.reserve(lvr2_vertex_normals.numElements()); - for (size_t i = 0; i < lvr2_vertex_normals.numElements(); i++) - { - m_normals.push_back(Normal(lvr2_vertex_normals[i][0], lvr2_vertex_normals[i][1], lvr2_vertex_normals[i][2])); - } - } - - - - - /// OLD - - // // Open file IO - // hdf5_map_io::HDF5MapIO map_io(mapFile); - - // Read geometry - - // m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + // TODO: Read textures/materials is missing + // Description: + // the old code was including loading texutures. However this was never tested for ROS 2 nor do we have any h5 files + // to test this (only h5 files with vertex colors). Therefore I removed it until we have a working test case. // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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++) @@ -485,29 +494,48 @@ bool MapDisplay::loadData() // m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); // } - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex colors"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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 + m_colors.clear(); + if(auto lvr2_vertex_colors_opt = mesh_buffer->getChannel("vertex_colors")) + { + lvr2::Channel lvr_vertex_colors = *lvr2_vertex_colors_opt; + m_colors.resize(lvr_vertex_colors.numElements()); + const bool has_alpha = lvr_vertex_colors.width() > 3; + for (size_t i = 0; i < lvr_vertex_colors.numElements(); i ++) + { + // convert from 0-255 (uint8) to 0.0-1.0 (float) + m_colors[i].r = lvr_vertex_colors[i][0] / 255.0f; + m_colors[i].g = lvr_vertex_colors[i][1] / 255.0f; + m_colors[i].b = lvr_vertex_colors[i][2] / 255.0f; + if(has_alpha) + { + m_colors[i].a = lvr_vertex_colors[i][3] / 255.0f; + } else { + m_colors[i].a = 1.0; + } + } + } - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex normals"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "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])); - // } + m_normals.clear(); + if(auto lvr2_vertex_normals_opt = mesh_buffer->getChannel("vertex_normals")) + { + lvr2::Channel lvr2_vertex_normals = *lvr2_vertex_normals_opt; + m_normals.reserve(lvr2_vertex_normals.numElements()); + for (size_t i = 0; i < lvr2_vertex_normals.numElements(); i++) + { + m_normals.push_back(Normal( + lvr2_vertex_normals[i][0], + lvr2_vertex_normals[i][1], + lvr2_vertex_normals[i][2])); + } + } + // Same problem as above: This is a piece of code we cannot test. Therefore I removed it. // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load texture coordinates"); // // Read tex cords @@ -521,6 +549,7 @@ bool MapDisplay::loadData() // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load clusters"); + // Same problem as above: This is a piece of code we cannot test. Therefore I removed it. // // Read labels // m_clusterList.clear(); // // m_clusterList.push_back(Cluster("__NEW__", vector())); @@ -538,18 +567,27 @@ bool MapDisplay::loadData() // } // } - // m_costs.clear(); - // for (std::string costlayer : map_io.getCostLayers()) - // { - // try - // { - // m_costs[costlayer] = map_io.getVertexCosts(costlayer); - // } - // catch (const hf::DataSpaceException& e) - // { - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); - // } - // } + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load costs"); + std::unordered_set channels_to_exclude = { + "vertices", + "vertex_normals" + }; + + m_costs.clear(); + for(auto elem : *mesh_buffer) + { + // this is checking if a channel is a vertex cost channel + // I suggest for future changes to somehow mark a channel as "vertex_cost" instead + if(elem.second.is_type() + && elem.second.numElements() == m_geometry->vertices.size() + && channels_to_exclude.find(elem.first) == channels_to_exclude.end() + && elem.second.width() == 1) + { + // vertex channel of type float + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Loading '" << elem.first << "' as vertex cost layer"); + m_costs[elem.first] = copy_1d_channel_to_vector(elem.second.extract()); + } + } } else { From 33104f2774db267667f6af222c8404435037cdb3 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 9 Dec 2024 16:46:13 +0100 Subject: [PATCH 3/4] last changes for having the same functionalities as before. removed mesh_msgs_hdf5 because it was nowhere used. --- mesh_msgs_conversions/CMakeLists.txt | 6 + mesh_msgs_hdf5/CHANGELOG.rst | 20 - mesh_msgs_hdf5/CMakeLists.txt | 66 --- mesh_msgs_hdf5/COLCON_IGNORE | 0 .../include/mesh_msgs_hdf5/mesh_msgs_hdf5.h | 122 ----- mesh_msgs_hdf5/package.xml | 29 - mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp | 509 ------------------ rviz_mesh_tools_plugins/CMakeLists.txt | 6 + 8 files changed, 12 insertions(+), 746 deletions(-) delete mode 100644 mesh_msgs_hdf5/CHANGELOG.rst delete mode 100644 mesh_msgs_hdf5/CMakeLists.txt delete mode 100644 mesh_msgs_hdf5/COLCON_IGNORE delete mode 100644 mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h delete mode 100644 mesh_msgs_hdf5/package.xml delete mode 100644 mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp diff --git a/mesh_msgs_conversions/CMakeLists.txt b/mesh_msgs_conversions/CMakeLists.txt index 12be500..f4f12ff 100644 --- a/mesh_msgs_conversions/CMakeLists.txt +++ b/mesh_msgs_conversions/CMakeLists.txt @@ -10,6 +10,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# Default: Release +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() +endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) diff --git a/mesh_msgs_hdf5/CHANGELOG.rst b/mesh_msgs_hdf5/CHANGELOG.rst deleted file mode 100644 index d8ce681..0000000 --- a/mesh_msgs_hdf5/CHANGELOG.rst +++ /dev/null @@ -1,20 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mesh_msgs_hdf5 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (2024-05-15) ------------------- -* port package to ROS2 - -1.1.0 (2021-10-27) ------------------- -* added publishing of mesh_msgs to mesh_msgs_hdf5 package -* fixed hdf5 to mesh_msgs publisher node -* resolved catkin lint problems - -1.0.1 (2020-11-11) ------------------- - -1.0.0 (2020-04-26) ------------------- -* release version 1.0.0 diff --git a/mesh_msgs_hdf5/CMakeLists.txt b/mesh_msgs_hdf5/CMakeLists.txt deleted file mode 100644 index 1cf8ef7..0000000 --- a/mesh_msgs_hdf5/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.8) - -project(mesh_msgs_hdf5) - - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(actionlib_msgs REQUIRED) -find_package(mesh_msgs REQUIRED) -find_package(hdf5_map_io REQUIRED) -find_package(label_manager REQUIRED) - -find_package(HDF5 REQUIRED COMPONENTS C CXX HL) - -# LVR2 includes HighFive that we need here -# - it was compiling before because globale lvr2 header were accessible -# find_package(LVR2 REQUIRED) - -include_directories( - include - ${HDF5_INCLUDE_DIRS} -) - -add_executable(${PROJECT_NAME} - src/mesh_msgs_hdf5.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${HDF5_LIBRARIES} - ${HDF5_HL_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - rclcpp_components - actionlib_msgs - mesh_msgs - hdf5_map_io - label_manager) - -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -ament_package() diff --git a/mesh_msgs_hdf5/COLCON_IGNORE b/mesh_msgs_hdf5/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h deleted file mode 100644 index cc5a1b0..0000000 --- a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h +++ /dev/null @@ -1,122 +0,0 @@ -#ifndef MESH_MSGS_HDF5_H_ -#define MESH_MSGS_HDF5_H_ - - -#include -#include -#include -#include -#include -#include - -#include - -#include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" -#include "mesh_msgs/srv/get_geometry.hpp" -#include "mesh_msgs/srv/get_materials.hpp" -#include "mesh_msgs/srv/get_texture.hpp" -#include "mesh_msgs/srv/get_uui_ds.hpp" // :D GetUUIDs -#include "mesh_msgs/srv/get_vertex_colors.hpp" -#include "mesh_msgs/srv/get_vertex_costs.hpp" -#include "mesh_msgs/srv/get_vertex_cost_layers.hpp" -#include "mesh_msgs/srv/get_labeled_clusters.hpp" -#include "label_manager/srv/get_label_groups.hpp" -#include "label_manager/srv/get_labeled_cluster_group.hpp" -#include "label_manager/srv/delete_label.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/fill_image.hpp" - -#include "rclcpp/rclcpp.hpp" - -namespace mesh_msgs_hdf5 -{ - -class hdf5_to_msg : public rclcpp::Node -{ - - public: - hdf5_to_msg(); - - protected: - void loadAndPublishGeometry(); - - bool getVertices(std::vector& vertices, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); - bool getFaces(std::vector& faceIds, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); - bool getVertexNormals(std::vector& vertexNormals, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); - - bool getVertexColors(std::vector& vertexColors, mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg); - bool getVertexCosts(std::vector& vertexCosts, std::string layer, mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg); - - // Mesh services - bool service_getUUIDs( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getGeometry( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getGeometryVertices( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getGeometryFaces( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getGeometryVertexNormals( - const std::shared_ptr req, - std::shared_ptr res); - - bool service_getMaterials( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getTexture( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getVertexColors( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getVertexCosts( - const std::shared_ptr req, - std::shared_ptr res); - bool service_getVertexCostLayers( - const std::shared_ptr req, - std::shared_ptr res); - - // Label manager services - bool service_getLabeledClusters( - const std::shared_ptr req, - std::shared_ptr res); - - void callback_clusterLabel(const mesh_msgs::msg::MeshFaceClusterStamped& msg); - - private: - - // Mesh message service servers - - rclcpp::Service::SharedPtr srv_get_uuids_; - rclcpp::Service::SharedPtr srv_get_geometry_; - rclcpp::Service::SharedPtr srv_get_geometry_vertices_; - rclcpp::Service::SharedPtr srv_get_geometry_faces_; - rclcpp::Service::SharedPtr srv_get_geometry_vertex_normals_; - rclcpp::Service::SharedPtr srv_get_materials_; - rclcpp::Service::SharedPtr srv_get_texture_; - rclcpp::Service::SharedPtr srv_get_vertex_colors_; - rclcpp::Service::SharedPtr srv_get_vertex_costs_; - rclcpp::Service::SharedPtr srv_get_vertex_cost_layers_; - - // Mesh message publishers - rclcpp::Publisher::SharedPtr pub_geometry_; - rclcpp::Publisher::SharedPtr pub_vertex_colors_; - rclcpp::Publisher::SharedPtr pub_vertex_costs_; - - // Label manager services and subs/pubs - rclcpp::Service::SharedPtr srv_get_labeled_clusters_; - rclcpp::Subscription::SharedPtr sub_cluster_label_; - - // ROS parameter - std::string inputFile; - - std::string mesh_uuid = "mesh"; -}; - -} // end namespace - -#endif /* MESH_MSGS_HDF5_H_ */ diff --git a/mesh_msgs_hdf5/package.xml b/mesh_msgs_hdf5/package.xml deleted file mode 100644 index 4ff5ec2..0000000 --- a/mesh_msgs_hdf5/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - mesh_msgs_hdf5 - 2.0.0 - Read mesh_msgs from hdf5 - Alexander Mock - Matthias Holoch - Sebastian Pütz - http://wiki.ros.org/ros_mesh_tools/mesh_msgs_hdf5 - BSD-3 - Sebastian Pütz - - ament_cmake - - rclcpp - rclcpp_action - rclcpp_components - actionlib_msgs - mesh_msgs - hdf5_map_io - label_manager - - ament_lint_common - - - ament_cmake - - diff --git a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp deleted file mode 100644 index 35347e0..0000000 --- a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp +++ /dev/null @@ -1,509 +0,0 @@ -#include -#include -#include - -#include "rclcpp/executors.hpp" -#include "rclcpp/executors/multi_threaded_executor.hpp" - -using std::placeholders::_1; -using std::placeholders::_2; - -namespace mesh_msgs_hdf5 -{ - -hdf5_to_msg::hdf5_to_msg() -:rclcpp::Node("mesh_msgs_hdf5") -{ - // TODO: check if this is correct - this->declare_parameter("inputFile", "/tmp/map.h5"); - inputFile = this->get_parameter("inputFile").as_string(); - - RCLCPP_INFO_STREAM(this->get_logger(), "Using input file: " << inputFile); - - srv_get_uuids_ = this->create_service( - "get_uuids", std::bind(&hdf5_to_msg::service_getUUIDs, this, _1, _2)); - srv_get_geometry_ = this->create_service( - "get_geometry", std::bind(&hdf5_to_msg::service_getGeometry, this, _1, _2)); - srv_get_geometry_vertices_ = this->create_service( - "get_geometry_vertices", std::bind(&hdf5_to_msg::service_getGeometryVertices, this, _1, _2)); - srv_get_geometry_faces_ = this->create_service( - "get_geometry_faces", std::bind(&hdf5_to_msg::service_getGeometryFaces, this, _1, _2)); - srv_get_geometry_vertex_normals_ = this->create_service( - "get_geometry_vertexnormals", std::bind(&hdf5_to_msg::service_getGeometryVertexNormals, this, _1, _2)); - srv_get_materials_ = this->create_service( - "get_materials", std::bind(&hdf5_to_msg::service_getMaterials, this, _1, _2)); - srv_get_texture_ = this->create_service( - "get_texture", std::bind(&hdf5_to_msg::service_getTexture, this, _1, _2)); - srv_get_vertex_colors_ = this->create_service( - "get_vertex_colors", std::bind(&hdf5_to_msg::service_getVertexColors, this, _1, _2)); - srv_get_vertex_costs_ = this->create_service( - "get_vertex_costs", std::bind(&hdf5_to_msg::service_getVertexCosts, this, _1, _2)); - srv_get_vertex_cost_layers_ = this->create_service( - "get_vertex_cost_layers", std::bind(&hdf5_to_msg::service_getVertexCostLayers, this, _1, _2)); - - pub_geometry_ = this->create_publisher( - "mesh/geometry", 1); - pub_vertex_colors_ = this->create_publisher( - "mesh/vertex_colors", 1); - pub_vertex_costs_ = this->create_publisher( - "mesh/vertex_costs", 1); - - - srv_get_labeled_clusters_ = this->create_service( - "get_labeled_clusters", std::bind(&hdf5_to_msg::service_getLabeledClusters, this, _1, _2)); - - sub_cluster_label_ = this->create_subscription( - "cluster_label", 10, std::bind(&hdf5_to_msg::callback_clusterLabel, this, _1)); - - - loadAndPublishGeometry(); -} - -void hdf5_to_msg::loadAndPublishGeometry() -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // geometry - mesh_msgs::msg::MeshGeometryStamped geometryMsg; - - auto vertices = io.getVertices(); - auto faceIds = io.getFaceIds(); - auto vertexNormals = io.getVertexNormals(); - - getVertices(vertices, geometryMsg); - getFaces(faceIds, geometryMsg); - getVertexNormals(vertexNormals, geometryMsg); - - pub_geometry_->publish(geometryMsg); - - // vertex colors - mesh_msgs::msg::MeshVertexColorsStamped vertexColorsMsg; - - auto vertexColors = io.getVertexColors(); - getVertexColors(vertexColors, vertexColorsMsg); - - pub_vertex_colors_->publish(vertexColorsMsg); - - // vertex costs - mesh_msgs::msg::MeshVertexCostsStamped vertexCostsMsg; - for (std::string costlayer : io.getCostLayers()) - { - try - { - auto costs = io.getVertexCosts(costlayer); - getVertexCosts(costs, costlayer, vertexCostsMsg); - - pub_vertex_costs_->publish(vertexCostsMsg); - } - catch (const hf::DataSpaceException& e) - { - RCLCPP_WARN_STREAM(this->get_logger(), "Could not load costlayer " << costlayer); - } - } -} - -bool hdf5_to_msg::getVertices( - std::vector& vertices, - mesh_msgs::msg::MeshGeometryStamped& geometryMsg) -{ - unsigned int nVertices = vertices.size() / 3; - RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices"); - geometryMsg.mesh_geometry.vertices.resize(nVertices); - for (unsigned int i = 0; i < nVertices; i++) - { - geometryMsg.mesh_geometry.vertices[i].x = vertices[i * 3]; - geometryMsg.mesh_geometry.vertices[i].y = vertices[i * 3 + 1]; - geometryMsg.mesh_geometry.vertices[i].z = vertices[i * 3 + 2]; - } - - // Header - geometryMsg.uuid = mesh_uuid; - geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::getFaces( - std::vector& faceIds, - mesh_msgs::msg::MeshGeometryStamped& geometryMsg) -{ - unsigned int nFaces = faceIds.size() / 3; - RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nFaces << " faces"); - geometryMsg.mesh_geometry.faces.resize(nFaces); - for (unsigned int i = 0; i < nFaces; i++) - { - geometryMsg.mesh_geometry.faces[i].vertex_indices[0] = faceIds[i * 3]; - geometryMsg.mesh_geometry.faces[i].vertex_indices[1] = faceIds[i * 3 + 1]; - geometryMsg.mesh_geometry.faces[i].vertex_indices[2] = faceIds[i * 3 + 2]; - } - - // Header - geometryMsg.uuid = mesh_uuid; - geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::getVertexNormals( - std::vector& vertexNormals, - mesh_msgs::msg::MeshGeometryStamped& geometryMsg) -{ - unsigned int nVertexNormals = vertexNormals.size() / 3; - RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertexNormals << " vertex normals"); - geometryMsg.mesh_geometry.vertex_normals.resize(nVertexNormals); - for (unsigned int i = 0; i < nVertexNormals; i++) - { - geometryMsg.mesh_geometry.vertex_normals[i].x = vertexNormals[i * 3]; - geometryMsg.mesh_geometry.vertex_normals[i].y = vertexNormals[i * 3 + 1]; - geometryMsg.mesh_geometry.vertex_normals[i].z = vertexNormals[i * 3 + 2]; - } - - // Header - geometryMsg.uuid = mesh_uuid; - geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::getVertexColors( - std::vector& vertexColors, - mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg) -{ - unsigned int nVertices = vertexColors.size() / 3; - RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices for vertex colors"); - vertexColorsMsg.mesh_vertex_colors.vertex_colors.resize(nVertices); - for (unsigned int i = 0; i < nVertices; i++) - { - vertexColorsMsg.mesh_vertex_colors - .vertex_colors[i].r = vertexColors[i * 3] / 255.0f; - vertexColorsMsg.mesh_vertex_colors - .vertex_colors[i].g = vertexColors[i * 3 + 1] / 255.0f; - vertexColorsMsg.mesh_vertex_colors - .vertex_colors[i].b = vertexColors[i * 3 + 2] / 255.0f; - vertexColorsMsg.mesh_vertex_colors - .vertex_colors[i].a = 1; - } - - // Header - vertexColorsMsg.uuid = mesh_uuid; - vertexColorsMsg.header.frame_id = "map"; - vertexColorsMsg.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::getVertexCosts( - std::vector& costs, - std::string layer, - mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg) -{ - vertexCostsMsg.mesh_vertex_costs.costs.resize(costs.size()); - for (uint32_t i = 0; i < costs.size(); i++) - { - vertexCostsMsg.mesh_vertex_costs.costs[i] = costs[i]; - } - - vertexCostsMsg.uuid = mesh_uuid; - vertexCostsMsg.type = layer; - vertexCostsMsg.header.frame_id = "map"; - vertexCostsMsg.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::service_getUUIDs( - const std::shared_ptr req, - std::shared_ptr res) -{ - res->uuids.push_back(mesh_uuid); - return true; -} - -bool hdf5_to_msg::service_getGeometry( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // Vertices - auto vertices = io.getVertices(); - getVertices(vertices, res->mesh_geometry_stamped); - - // Faces - auto faceIds = io.getFaceIds(); - getFaces(faceIds, res->mesh_geometry_stamped); - - // Vertex normals - auto vertexNormals = io.getVertexNormals(); - getVertexNormals(vertexNormals, res->mesh_geometry_stamped); - - return true; -} - -bool hdf5_to_msg::service_getGeometryVertices( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // Vertices - auto vertices = io.getVertices(); - return getVertices(vertices, res->mesh_geometry_stamped); -} - -bool hdf5_to_msg::service_getGeometryFaces( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // Faces - auto faceIds = io.getFaceIds(); - return getFaces(faceIds, res->mesh_geometry_stamped); -} - -bool hdf5_to_msg::service_getGeometryVertexNormals( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // Vertex normals - auto vertexNormals = io.getVertexNormals(); - return getVertexNormals(vertexNormals, res->mesh_geometry_stamped); -} - -bool hdf5_to_msg::service_getMaterials( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - - // Materials - auto materials = io.getMaterials(); - auto materialFaceIndices = io.getMaterialFaceIndices(); // for each face: material index - unsigned int nMaterials = materials.size(); - unsigned int nFaces = materialFaceIndices.size(); - RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nMaterials << " materials and " << nFaces << " faces"); - res->mesh_materials_stamped.mesh_materials.materials.resize(nMaterials); - for (uint32_t i = 0; i < nMaterials; i++) - { - int texture_index = materials[i].textureIndex; - - // has texture - res->mesh_materials_stamped - .mesh_materials - .materials[i] - .has_texture = texture_index >= 0; - - // texture index - res->mesh_materials_stamped.mesh_materials.materials[i] - .texture_index = static_cast(texture_index); - // color - res->mesh_materials_stamped.mesh_materials.materials[i] - .color.r = materials[i].r / 255.0f; - res->mesh_materials_stamped.mesh_materials.materials[i] - .color.g = materials[i].g / 255.0f; - res->mesh_materials_stamped.mesh_materials.materials[i] - .color.b = materials[i].b / 255.0f; - res->mesh_materials_stamped.mesh_materials.materials[i] - .color.a = 1; - } - - // Clusters - // Map materials to face IDs - std::map> materialToFaces; - // Iterate over face <> material index - for (uint32_t i = 0; i < nFaces; i++) - { - // material index for face i - uint32_t materialIndex = materialFaceIndices[i]; - - if (materialToFaces.count(materialIndex) == 0) - { - materialToFaces.insert(std::make_pair(materialIndex, std::vector())); - } - - materialToFaces[materialIndex].push_back(i); - } - // For each material, map contains a list of faces - res->mesh_materials_stamped.mesh_materials.clusters.resize(nMaterials); - for (uint32_t i = 0; i < nMaterials; i++) - { - for (uint32_t j = 0; j < materialToFaces[i].size(); j++) - { - res->mesh_materials_stamped.mesh_materials.clusters[i].face_indices.push_back(materialToFaces[i][j]); - } - } - res->mesh_materials_stamped.mesh_materials.cluster_materials.resize(nMaterials); - for (uint32_t i = 0; i < nMaterials; i++) - { - res->mesh_materials_stamped.mesh_materials.cluster_materials[i] = i; - } - - // Vertex Tex Coords - auto vertexTexCoords = io.getVertexTextureCoords(); - unsigned int nVertices = vertexTexCoords.size() / 3; - res->mesh_materials_stamped.mesh_materials.vertex_tex_coords.resize(nVertices); - for (uint32_t i = 0; i < nVertices; i++) - { - // coords: u/v/w - // w is always 0 - res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].u = vertexTexCoords[3 * i]; - res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].v = vertexTexCoords[3 * i + 1]; - } - - // Header - res->mesh_materials_stamped.uuid = mesh_uuid; - res->mesh_materials_stamped.header.frame_id = "map"; - res->mesh_materials_stamped.header.stamp = this->get_clock()->now(); - - return true; -} - -bool hdf5_to_msg::service_getTexture( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - for (auto texture : io.getTextures()) - { - if (std::stoi(texture.name) == req->texture_index) - { - res->texture.texture_index = req->texture_index; - res->texture.uuid = mesh_uuid; - sensor_msgs::msg::Image image; - sensor_msgs::fillImage( // TODO: only RGB, breaks when using other color channels - image, - "rgb8", - texture.height, - texture.width, - texture.width * 3, // step size - texture.data.data() - ); - - res->texture.image = image; - - return true; - } - } - - return false; -} - -bool hdf5_to_msg::service_getVertexColors( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // Vertex colors - auto vertexColors = io.getVertexColors(); - return getVertexColors(vertexColors, res->mesh_vertex_colors_stamped); -} - -bool hdf5_to_msg::service_getVertexCosts( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - auto costs = io.getVertexCosts(req->layer); - return getVertexCosts(costs, req->layer, res->mesh_vertex_costs_stamped); -} - -bool hdf5_to_msg::service_getVertexCostLayers( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - res->layers = io.getCostLayers(); - return true; -} - -bool hdf5_to_msg::service_getLabeledClusters( - const std::shared_ptr req, - std::shared_ptr res) -{ - hdf5_map_io::HDF5MapIO io(inputFile); - - // iterate over groups - auto groups = io.getLabelGroups(); - for (size_t i = 0; i < groups.size(); i++) - { - // iterate over labels in group - auto labelsInGroup = io.getAllLabelsOfGroup(groups[i]); - for (size_t j = 0; j < labelsInGroup.size(); j++) - { - // copy label - auto faceIds = io.getFaceIdsOfLabel(groups[i], labelsInGroup[j]); - mesh_msgs::msg::MeshFaceCluster cluster; - std::stringstream ss; - ss << groups[i] << "_" << labelsInGroup[j]; - cluster.label = ss.str(); - cluster.face_indices.resize(faceIds.size()); - for (size_t k = 0; k < faceIds.size(); k++) - { - cluster.face_indices[k] = faceIds[k]; - } - res->clusters.push_back(cluster); - } - } - - return true; -} - -void hdf5_to_msg::callback_clusterLabel( - const mesh_msgs::msg::MeshFaceClusterStamped& msg) -{ - if (msg.uuid.compare(mesh_uuid) != 0) - { - RCLCPP_ERROR(this->get_logger(), "Invalid mesh UUID"); - return; - } - - hdf5_map_io::HDF5MapIO io(inputFile); - - // TODO: implement optional override - RCLCPP_WARN(this->get_logger(), "Override is enabled by default"); - - // split label id into group and name - std::vector split_results; - boost::split(split_results, msg.cluster.label, [](char c){ return c == '_'; }); - - if (split_results.size() != 2) - { - RCLCPP_ERROR(this->get_logger(), "Received illegal cluster name"); - return; - } - - - std::string label_group = split_results[0]; - std::string label_name = split_results[1]; - std::vector indices; - for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) - { - indices.push_back(msg.cluster.face_indices[i]); - } - - // write to hdf5 - io.addLabel(label_group, label_name, indices); -} - -} // namespace mesh_msgs_hdf5 - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::ExecutorOptions opts; - rclcpp::executors::MultiThreadedExecutor executor(opts, 4); - executor.add_node(std::make_shared()); - executor.spin(); - return 0; -} diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index bf0987b..7e3609e 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -10,6 +10,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() +endif() + add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") From 3de31c622b8b1e006ba6dd31c701435d6212a58d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 12 Dec 2024 00:52:40 +0100 Subject: [PATCH 4/4] properly inititalized for loop id. added std namespace to strings --- mesh_msgs_conversions/src/conversions.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/mesh_msgs_conversions/src/conversions.cpp b/mesh_msgs_conversions/src/conversions.cpp index 16a9e65..4830dd3 100644 --- a/mesh_msgs_conversions/src/conversions.cpp +++ b/mesh_msgs_conversions/src/conversions.cpp @@ -239,11 +239,10 @@ bool fromMeshGeometryToMeshBuffer( const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBuffer& buffer) { - const size_t numVertices = mesh_geometry.vertices.size(); lvr2::floatArr vertices( new float[ numVertices * 3 ] ); const auto& mg_vertices = mesh_geometry.vertices; - for(size_t i; i(mg_vertices[i].x); vertices[ i * 3 + 1 ] = static_cast(mg_vertices[i].y); @@ -254,7 +253,7 @@ bool fromMeshGeometryToMeshBuffer( const size_t numFaces = mesh_geometry.faces.size(); lvr2::indexArray faces( new unsigned int[ numVertices * 3 ] ); const auto& mg_faces = mesh_geometry.faces; - for(size_t i; i(mg_normals[i].x); normals[ i * 3 + 1 ] = static_cast(mg_normals[i].y); @@ -276,7 +275,7 @@ bool fromMeshGeometryToMeshBuffer( return true; } -bool readMeshBuffer(lvr2::MeshBufferPtr& buffer_ptr, string path) +bool readMeshBuffer(lvr2::MeshBufferPtr& buffer_ptr, std::string path) { lvr2::ModelFactory io_factory; lvr2::ModelPtr model = io_factory.readModel(path); @@ -292,7 +291,7 @@ bool readMeshBuffer(lvr2::MeshBufferPtr& buffer_ptr, string path) } } -bool writeMeshBuffer(lvr2::MeshBufferPtr& buffer, string path) +bool writeMeshBuffer(lvr2::MeshBufferPtr& buffer, std::string path) { lvr2::ModelPtr model(new lvr2::Model(buffer)); lvr2::ModelFactory::saveModel(model, path);