From 6367eb24df4511235fb3faddbb9fbdc2ba3451dc Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 14 Nov 2024 23:17:19 +0100 Subject: [PATCH 01/17] Cleanup of mesh map: - removed some layer dependent variables - removed server connection. It can be readded at a later point. But right now it is old code and we don't know how it works. So for now I think it's better to remove it. --- mesh_map/src/mesh_map.cpp | 73 ++++++++++++--------------------------- 1 file changed, 22 insertions(+), 51 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 299d2ecb..58a9f5eb 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -59,8 +59,8 @@ namespace mesh_map { -using HDF5MeshIO = lvr2::Hdf5IO; + +using HDF5MeshIO = lvr2::Hdf5Build; MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) : tf_buffer(tf_buffer) @@ -70,21 +70,6 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) , layer_loader("mesh_map", "mesh_map::AbstractLayer") , mesh_ptr(new lvr2::HalfEdgeMesh()) { - srv_url = node->declare_parameter(MESH_MAP_NAMESPACE + ".server_url", ""); - srv_username = node->declare_parameter(MESH_MAP_NAMESPACE + ".server_username", ""); - srv_password = node->declare_parameter(MESH_MAP_NAMESPACE + ".server_password", ""); - mesh_layer = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_layer", "mesh0"); - min_roughness = node->declare_parameter(MESH_MAP_NAMESPACE + ".min_roughness", 0.0); - max_roughness = node->declare_parameter(MESH_MAP_NAMESPACE + ".max_roughness", 0.0); - min_height_diff = node->declare_parameter(MESH_MAP_NAMESPACE + ".min_height_diff", 0.0); - max_height_diff = node->declare_parameter(MESH_MAP_NAMESPACE + ".max_height_diff", 0.0); - bb_min_x = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_min_x", 0.0); - bb_min_y = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_min_y", 0.0); - bb_min_z = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_min_z", 0.0); - bb_max_x = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_max_x", 0.0); - bb_max_y = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_max_y", 0.0); - bb_max_z = node->declare_parameter(MESH_MAP_NAMESPACE + ".bb_max_z", 0.0); - auto min_contour_size_desc = rcl_interfaces::msg::ParameterDescriptor{}; min_contour_size_desc.name = MESH_MAP_NAMESPACE + ".min_contour_size"; min_contour_size_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; @@ -95,7 +80,7 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) min_contour_size_desc.integer_range.push_back(min_contour_size_range); min_contour_size = node->declare_parameter(MESH_MAP_NAMESPACE + ".min_contour_size", 3); - auto layer_factor_desc = rcl_interfaces::msg::ParameterDescriptor{}; + auto layer_factor_desc = rcl_interfaces::msg::ParameterDescriptor{}; layer_factor_desc.name = MESH_MAP_NAMESPACE + ".layer_factor"; layer_factor_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; layer_factor_desc.description = "Defines the factor for combining edge distances and vertex costs."; @@ -156,45 +141,32 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) bool MeshMap::readMap() { RCLCPP_INFO_STREAM(node->get_logger(), "server url: " << srv_url); - bool server = false; - - if (!srv_url.empty()) - { - server = true; - - mesh_io_ptr = std::shared_ptr( - new mesh_client::MeshClient(srv_url, srv_username, srv_password, mesh_layer)); - auto mesh_client_ptr = std::static_pointer_cast(mesh_io_ptr); - - mesh_client_ptr->setBoundingBox(bb_min_x, bb_min_y, bb_min_z, bb_max_x, bb_max_y, bb_max_z); - mesh_client_ptr->addFilter("roughness", min_roughness, max_roughness); - mesh_client_ptr->addFilter("height_diff", min_height_diff, max_height_diff); - } - else if (!mesh_file.empty() && !mesh_part.empty()) + + if (!mesh_file.empty() && !mesh_part.empty()) { + // check file ending RCLCPP_INFO_STREAM(node->get_logger(), "Load \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); - HDF5MeshIO* hdf_5_mesh_io = new HDF5MeshIO(); - hdf_5_mesh_io->open(mesh_file); - hdf_5_mesh_io->setMeshName(mesh_part); - mesh_io_ptr = std::shared_ptr(hdf_5_mesh_io); + + + // HDF5MeshIO* hdf_5_mesh_io = new HDF5MeshIO(); + // hdf_5_mesh_io->open(mesh_file); + // hdf_5_mesh_io->setMeshName(mesh_part); + // mesh_io_ptr = std::shared_ptr(hdf_5_mesh_io); + + auto hdf5_mesh_io = std::make_shared(); + hdf5_mesh_io->open(mesh_file); + hdf5_mesh_io->setMeshName(mesh_part); + mesh_io_ptr = hdf5_mesh_io; } else { - RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open file or server connection!"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open file connection!"); return false; } - if (server) - { - RCLCPP_INFO_STREAM(node->get_logger(), "Start reading the mesh from the server '" << srv_url); - } - else - { - RCLCPP_INFO_STREAM(node->get_logger(), "Start reading the mesh part '" << mesh_part << "' from the map file '" << mesh_file << "'..."); - } + RCLCPP_INFO_STREAM(node->get_logger(), "Start reading the mesh part '" << mesh_part << "' from the map file '" << mesh_file << "'..."); auto mesh_opt = mesh_io_ptr->getMesh(); - if (mesh_opt) { *mesh_ptr = mesh_opt.get(); @@ -539,12 +511,11 @@ void MeshMap::findContours(std::vector>& contour // If border Edge found if ((!facepair[0] || !facepair[1]) && !usedEdges[eHStart]) { - std: - vector contour; + std::vector contour; // Set vector which links to the following Edge - array vertexPair = mesh_ptr->getVerticesOfEdge(eHStart); + std::array vertexPair = mesh_ptr->getVerticesOfEdge(eHStart); lvr2::VertexHandle vH = vertexPair[1]; - vector curEdges; + std::vector curEdges; lvr2::EdgeHandle eHTemp = eHStart; bool moving = true; bool vertex_flag = false; From edd9cb2e43b2177001b7320bf969a706600e106e Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 00:40:07 +0100 Subject: [PATCH 02/17] added template for generic mesh file loading --- mesh_map/CMakeLists.txt | 7 +- mesh_map/include/mesh_map/mesh_map.h | 37 ++++----- mesh_map/include/mesh_map/util.h | 5 ++ mesh_map/package.xml | 1 + mesh_map/src/mesh_map.cpp | 107 +++++++++++++++++++++------ mesh_map/src/util.cpp | 24 ++++++ 6 files changed, 130 insertions(+), 51 deletions(-) diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index 36245644..3b6f8224 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -19,9 +19,8 @@ foreach(dep IN LISTS dependencies) endforeach() find_package(LVR2 REQUIRED) +find_package(assimp REQUIRED) find_package(MPI) -# this is nowhere used -# find_package(Boost REQUIRED COMPONENTS system) find_package(PkgConfig REQUIRED) pkg_check_modules(JSONCPP jsoncpp) @@ -36,10 +35,12 @@ add_library(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC $ $ - ${LVR2_INCLUDE_DIRS}) + ${LVR2_INCLUDE_DIRS} + ${ASSIMP_INCLUDE_DIRS}) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) target_link_libraries(${PROJECT_NAME} ${LVR2_LIBRARIES} + ${ASSIMP_LIBRARIES} ${MPI_CXX_LIBRARIES} ) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index d6c60c60..7b6d9d67 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -398,11 +398,14 @@ class MeshMap */ mesh_map::AbstractLayer::Ptr layer(const std::string& layer_name); + //! This is an abstract interface to load mesh information from somewhere + //! The most default case is loading from a HDF5 file + //! However we could also implement a server connection here + //! We might use the pluginlib for that std::shared_ptr mesh_io_ptr; std::shared_ptr> mesh_ptr; lvr2::DenseVertexMap invalid; - private: //! plugin class loader for for the layer plugins pluginlib::ClassLoader layer_loader; @@ -424,31 +427,17 @@ class MeshMap //! global frame / coordinate system id std::string global_frame; - //! server url - std::string srv_url; - - //! login username to connect to the server - std::string srv_username; - - //! login password to connect to the server - std::string srv_password; - - std::string mesh_layer; - - double min_roughness; - double max_roughness; - double min_height_diff; - double max_height_diff; - double bb_min_x; - double bb_min_y; - double bb_min_z; - double bb_max_x; - double bb_max_y; - double bb_max_z; - - + //! mesh file std::string mesh_file; + + //! mesh part std::string mesh_part; + + //! mesh working file + std::string mesh_working_file; + + //! mesh working part + std::string mesh_working_part; //! dynamic params callback handle rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr config_callback; diff --git a/mesh_map/include/mesh_map/util.h b/mesh_map/include/mesh_map/util.h index df698aab..71a7c59c 100644 --- a/mesh_map/include/mesh_map/util.h +++ b/mesh_map/include/mesh_map/util.h @@ -48,6 +48,9 @@ #include #include +#include +#include + namespace mesh_map { @@ -58,6 +61,8 @@ typedef lvr2::Normal Normal; typedef lvr2::BaseVector Vector; +lvr2::MeshBufferPtr extractMeshByName(const aiScene* ascene, std::string name); + /** * @brief Function to build std_msgs color instances * @param r red, value between 0 and 1 diff --git a/mesh_map/package.xml b/mesh_map/package.xml index 9e64edae..d2978ae9 100644 --- a/mesh_map/package.xml +++ b/mesh_map/package.xml @@ -17,6 +17,7 @@ tf2_ros tf2 visualization_msgs + assimp ament_cmake_gmock diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 58a9f5eb..fea7efd5 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -57,6 +57,14 @@ #include #include +#include + +#include +#include +#include + +namespace fs = std::filesystem; + namespace mesh_map { @@ -101,6 +109,7 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) cost_limit = node->declare_parameter(MESH_MAP_NAMESPACE + ".cost_limit", 1.0); mesh_file = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_file", ""); + mesh_part = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_part", ""); global_frame = node->declare_parameter(MESH_MAP_NAMESPACE + ".global_frame", "map"); RCLCPP_INFO_STREAM(node->get_logger(), "mesh file is set to: " << mesh_file); @@ -139,29 +148,76 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) } bool MeshMap::readMap() -{ - RCLCPP_INFO_STREAM(node->get_logger(), "server url: " << srv_url); - - if (!mesh_file.empty() && !mesh_part.empty()) - { - // check file ending - RCLCPP_INFO_STREAM(node->get_logger(), "Load \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); - - - // HDF5MeshIO* hdf_5_mesh_io = new HDF5MeshIO(); - // hdf_5_mesh_io->open(mesh_file); - // hdf_5_mesh_io->setMeshName(mesh_part); - // mesh_io_ptr = std::shared_ptr(hdf_5_mesh_io); - - auto hdf5_mesh_io = std::make_shared(); - hdf5_mesh_io->open(mesh_file); - hdf5_mesh_io->setMeshName(mesh_part); - mesh_io_ptr = hdf5_mesh_io; - } - else +{ + if(!mesh_io_ptr) { - RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open file connection!"); - return false; + if(!mesh_file.empty() && !mesh_part.empty()) + { + // default: mesh_working_file = mesh_file + if(mesh_working_file == "") + { + mesh_working_file = fs::path(mesh_file).replace_extension(".h5"); + } + + if(mesh_working_part == "") + { + mesh_working_part == mesh_part; + } + + if(fs::path(mesh_working_file).extension() != ".h5") + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Working File has to be of type HDF5!"); + return false; + } + + // directly work on the input file + RCLCPP_INFO_STREAM(node->get_logger(), "Load \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); + auto hdf5_mesh_io = std::make_shared(); + hdf5_mesh_io->open(mesh_working_file); + hdf5_mesh_io->setMeshName(mesh_working_part); + mesh_io_ptr = hdf5_mesh_io; + + if(mesh_file != mesh_working_file) + { + lvr2::MeshBufferPtr mesh_buffer; + + // we have to create the working h5 first + if(fs::path(mesh_file).extension() == ".h5") + { + auto hdf5_mesh_input = std::make_shared(); + hdf5_mesh_input->open(mesh_file); + hdf5_mesh_input->setMeshName(mesh_part); + mesh_buffer = hdf5_mesh_input->MeshIO::load(mesh_part); + } else { + // use another loader + Assimp::Importer io; + io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); + const aiScene* ascene = io.ReadFile(mesh_file, aiProcess_PreTransformVertices | aiProcess_Triangulate | aiProcess_SortByPType); + if (!ascene) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Error while loading map: " << io.GetErrorString()); + return false; + } + mesh_buffer = extractMeshByName(ascene, mesh_part); + } + + if(!mesh_buffer) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Couldn't load mesh part: " << mesh_part); + } + + // write + hdf5_mesh_io->save(mesh_working_part, mesh_buffer); + } + + } + else + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open file connection!"); + return false; + } + } else { + RCLCPP_INFO_STREAM(node->get_logger(), "Connection to file exists already!"); } RCLCPP_INFO_STREAM(node->get_logger(), "Start reading the mesh part '" << mesh_part << "' from the map file '" << mesh_file << "'..."); @@ -408,7 +464,9 @@ void MeshMap::combineVertexCosts(const rclcpp::Time& map_stamp) float min, max; mesh_map::getMinMax(costs, min, max); const float norm = max - min; - const float factor = 1.0; // TODO how to declare param for each plugin? Needs to be done after plugins are loaded, which happens when the map gets loaded. Who calls readMap() and when? + const float factor = 1.0; + // TODO how to declare param for each plugin? + // Needs to be done after plugins are loaded, which happens when the map gets loaded. Who calls readMap() and when? // const float factor = private_nh.param(MESH_MAP_NAMESPACE + "/" + layer.first + "/factor", 1.0); const float norm_factor = factor / norm; RCLCPP_INFO_STREAM(node->get_logger(), "Layer \"" << layer.first << "\" max value: " << max << " min value: " << min << " norm: " << norm @@ -1198,7 +1256,8 @@ void MeshMap::publishVertexColors(const rclcpp::Time& map_stamp) } } -rcl_interfaces::msg::SetParametersResult MeshMap::reconfigureCallback(std::vector parameters) +rcl_interfaces::msg::SetParametersResult MeshMap::reconfigureCallback( + std::vector parameters) { RCLCPP_DEBUG_STREAM(node->get_logger(), "Set parameters callback..."); rcl_interfaces::msg::SetParametersResult result; diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index e7cd28a3..d8990ef1 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -44,6 +44,30 @@ namespace mesh_map { + +lvr2::MeshBufferPtr extractMeshByName( + const aiScene* ascene, + std::string name) +{ + lvr2::MeshBufferPtr mesh; + + // search for mesh part, if there are multiple meshes + for (unsigned int meshIdx = 0; meshIdx < ascene->mNumMeshes; meshIdx++) + { + // const aiMesh* amesh = ascene->mMeshes[meshIdx]; + + // // skip non-triangle meshes + // if (amesh->mPrimitiveTypes != aiPrimitiveType_TRIANGLE) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Mesh " << meshIdx << " is not a triangle mesh! Skipping..."); + // continue; + // } + + } + + return mesh; +} + void getMinMax(const lvr2::VertexMap& costs, float& min, float& max) { max = std::numeric_limits::min(); From 6324829bfff7c186764da7a6e35b174dfb232c53 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 01:18:09 +0100 Subject: [PATCH 03/17] Added conversion from assimp scene to lvr meshbuffer --- mesh_map/src/util.cpp | 101 +++++++++++++++++++++++++++++++++++++----- 1 file changed, 91 insertions(+), 10 deletions(-) diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index d8990ef1..04c523fa 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -45,24 +45,105 @@ namespace mesh_map { +const aiNode* extractNodeByName( + const aiNode* node, + std::string name) +{ + if(node == nullptr) + { + return nullptr; + } + + if(node->mNumMeshes > 0) + { + // found something that is a mesh + std::string node_name = node->mName.C_Str(); + if(node_name == name) + { + // found! + return node; + } + } + + for(size_t i=0; imNumChildren; i++) + { + const aiNode* child_node = extractNodeByName(node->mChildren[i], name); + if(child_node != nullptr) + { + return child_node; + } + } + + return nullptr; +} + lvr2::MeshBufferPtr extractMeshByName( const aiScene* ascene, std::string name) { lvr2::MeshBufferPtr mesh; - // search for mesh part, if there are multiple meshes - for (unsigned int meshIdx = 0; meshIdx < ascene->mNumMeshes; meshIdx++) + + const aiNode* root_node = ascene->mRootNode; + const aiNode* mesh_part_node = extractNodeByName(root_node, name); + + if(mesh_part_node) { - // const aiMesh* amesh = ascene->mMeshes[meshIdx]; - - // // skip non-triangle meshes - // if (amesh->mPrimitiveTypes != aiPrimitiveType_TRIANGLE) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Mesh " << meshIdx << " is not a triangle mesh! Skipping..."); - // continue; - // } + if(mesh_part_node->mNumMeshes > 1) + { + std::cout << "WARNING: the found mesh part consists of multiple meshes! Using the first one." << std::endl; + } + + unsigned int mesh_id = mesh_part_node->mMeshes[0]; + + const aiMesh* amesh = ascene->mMeshes[mesh_id]; + + // fill this + mesh = std::make_shared(); + + lvr2::Channel vertices(amesh->mNumVertices, 3); + for(size_t i=0; imNumVertices; i++) + { + vertices[i][0] = amesh->mVertices[i].x; + vertices[i][1] = amesh->mVertices[i].y; + vertices[i][2] = amesh->mVertices[i].z; + } + (*mesh)["vertices"] = vertices; + + lvr2::Channel face_indices(amesh->mNumFaces, 3); + for(size_t i=0; imNumFaces; i++) + { + face_indices[i][0] = amesh->mFaces[i].mIndices[0]; + face_indices[i][1] = amesh->mFaces[i].mIndices[1]; + face_indices[i][2] = amesh->mFaces[i].mIndices[2]; + } + (*mesh)["face_indices"] = face_indices; + if(amesh->HasNormals()) + { + // vertex normals + lvr2::Channel vertex_normals(amesh->mNumVertices, 3); + for(size_t i=0; imNumVertices; i++) + { + vertex_normals[i][0] = amesh->mNormals[i].x; + vertex_normals[i][1] = amesh->mNormals[i].y; + vertex_normals[i][2] = amesh->mNormals[i].z; + } + (*mesh)["vertex_normals"] = vertex_normals; + } + + if(amesh->HasVertexColors(0)) + { + lvr2::Channel vertex_colors(amesh->mNumVertices, 4); + for(size_t i=0; imNumVertices; i++) + { + vertex_colors[i][0] = amesh->mColors[0][i].r; + vertex_colors[i][1] = amesh->mColors[0][i].g; + vertex_colors[i][2] = amesh->mColors[0][i].b; + vertex_colors[i][3] = amesh->mColors[0][i].a; + } + (*mesh)["vertex_colors"] = vertex_colors; + } } return mesh; From f1ed54ab52673a6e634667540b0bee730bbefc49 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 01:33:47 +0100 Subject: [PATCH 04/17] added todos --- mesh_map/src/mesh_map.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index fea7efd5..91363985 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -109,9 +109,9 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) cost_limit = node->declare_parameter(MESH_MAP_NAMESPACE + ".cost_limit", 1.0); mesh_file = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_file", ""); - mesh_part = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_part", ""); global_frame = node->declare_parameter(MESH_MAP_NAMESPACE + ".global_frame", "map"); + RCLCPP_INFO_STREAM(node->get_logger(), "mesh file is set to: " << mesh_file); // params for map layer names to types: @@ -171,7 +171,8 @@ bool MeshMap::readMap() } // directly work on the input file - RCLCPP_INFO_STREAM(node->get_logger(), "Load \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); + RCLCPP_INFO_STREAM(node->get_logger(), "Connect to \"" << mesh_working_part << "\" from file \"" << mesh_working_file << "\"..."); + auto hdf5_mesh_io = std::make_shared(); hdf5_mesh_io->open(mesh_working_file); hdf5_mesh_io->setMeshName(mesh_working_part); @@ -179,6 +180,9 @@ bool MeshMap::readMap() if(mesh_file != mesh_working_file) { + RCLCPP_INFO_STREAM(node->get_logger(), "Initially loading \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); + + std::cout << "Generate seperate working file..." << std::endl; lvr2::MeshBufferPtr mesh_buffer; // we have to create the working h5 first @@ -188,6 +192,7 @@ bool MeshMap::readMap() hdf5_mesh_input->open(mesh_file); hdf5_mesh_input->setMeshName(mesh_part); mesh_buffer = hdf5_mesh_input->MeshIO::load(mesh_part); + // TODO: load all attributes? } else { // use another loader Assimp::Importer io; @@ -209,7 +214,6 @@ bool MeshMap::readMap() // write hdf5_mesh_io->save(mesh_working_part, mesh_buffer); } - } else { From ea31a4d111ac657fa8eecdca19050f1f30030ea3 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 17:37:29 +0100 Subject: [PATCH 05/17] loaded geometry from a common mesh file format --- mesh_map/src/mesh_map.cpp | 11 ++- mesh_map/src/util.cpp | 192 +++++++++++++++++++++++++------------- 2 files changed, 138 insertions(+), 65 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 91363985..14288d1e 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -110,6 +110,8 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) mesh_file = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_file", ""); mesh_part = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_part", ""); + mesh_working_file = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_working_file", ""); + mesh_working_part = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_working_part", ""); global_frame = node->declare_parameter(MESH_MAP_NAMESPACE + ".global_frame", "map"); RCLCPP_INFO_STREAM(node->get_logger(), "mesh file is set to: " << mesh_file); @@ -161,7 +163,11 @@ bool MeshMap::readMap() if(mesh_working_part == "") { - mesh_working_part == mesh_part; + std::cout << "BLABLAL: " << mesh_part << std::endl; + mesh_working_part = mesh_part; + RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part es default: '" << mesh_working_part << "'"); + } else { + RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is *not* empty: '" << mesh_working_part << "'"); } if(fs::path(mesh_working_file).extension() != ".h5") @@ -171,6 +177,7 @@ bool MeshMap::readMap() } // directly work on the input file + RCLCPP_INFO_STREAM(node->get_logger(), "Connect to \"" << mesh_working_part << "\" from file \"" << mesh_working_file << "\"..."); auto hdf5_mesh_io = std::make_shared(); @@ -197,7 +204,7 @@ bool MeshMap::readMap() // use another loader Assimp::Importer io; io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); - const aiScene* ascene = io.ReadFile(mesh_file, aiProcess_PreTransformVertices | aiProcess_Triangulate | aiProcess_SortByPType); + const aiScene* ascene = io.ReadFile(mesh_file, 0); if (!ascene) { RCLCPP_ERROR_STREAM(node->get_logger(), "Error while loading map: " << io.GetErrorString()); diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index 04c523fa..de03957f 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -42,35 +42,54 @@ #include #include +#include + +namespace fs = std::filesystem; + namespace mesh_map { -const aiNode* extractNodeByName( - const aiNode* node, - std::string name) +// again this function +std::vector split(std::string s, const std::string& delimiter) { - if(node == nullptr) + std::vector tokens; + size_t pos = 0; + std::string token; + while((pos = s.find(delimiter)) != std::string::npos) { - return nullptr; + token = s.substr(0, pos); + tokens.push_back(token); + s.erase(0, pos + delimiter.length()); } + tokens.push_back(s); - if(node->mNumMeshes > 0) + return tokens; +} + +const aiNode* getChildByName(const aiNode* node, std::string name) +{ + for(size_t i=0; imNumChildren; i++) { - // found something that is a mesh - std::string node_name = node->mName.C_Str(); - if(node_name == name) + const std::string child_name = node->mChildren[i]->mName.C_Str(); + if(child_name == name) { - // found! - return node; + return node->mChildren[i]; } } - - for(size_t i=0; imNumChildren; i++) + + return nullptr; +} + +const aiMesh* getMeshByName(const aiNode* node, aiMesh** meshes, std::string name) +{ + for(size_t i=0; imNumMeshes; i++) { - const aiNode* child_node = extractNodeByName(node->mChildren[i], name); - if(child_node != nullptr) + unsigned int mesh_id = node->mMeshes[i]; + const aiMesh* mesh = meshes[mesh_id]; + std::string mesh_name = mesh->mName.C_Str(); + if(mesh_name == name) { - return child_node; + return mesh; } } @@ -85,66 +104,113 @@ lvr2::MeshBufferPtr extractMeshByName( const aiNode* root_node = ascene->mRootNode; - const aiNode* mesh_part_node = extractNodeByName(root_node, name); - if(mesh_part_node) - { - if(mesh_part_node->mNumMeshes > 1) - { - std::cout << "WARNING: the found mesh part consists of multiple meshes! Using the first one." << std::endl; - } - unsigned int mesh_id = mesh_part_node->mMeshes[0]; - - const aiMesh* amesh = ascene->mMeshes[mesh_id]; + // ascene->mMe + + // const aiNode* mesh_part_node = extractNodeByName(root_node, name); + + // bool path_existing = false; + // transform from mesh to world + aiMatrix4x4 Tmw; + + // (T1 * T2) * T3 * p; + std::vector path_to_mesh = split(name, "/"); - // fill this - mesh = std::make_shared(); + if(path_to_mesh.size() == 0) + { + return mesh; + } + + const aiNode* node_it = root_node; + for(size_t i=1; i vertices(amesh->mNumVertices, 3); - for(size_t i=0; imNumVertices; i++) + if(node_it == nullptr) { - vertices[i][0] = amesh->mVertices[i].x; - vertices[i][1] = amesh->mVertices[i].y; - vertices[i][2] = amesh->mVertices[i].z; + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "'" << path_to_mesh[i] << "' not found in input mesh file!"); + break; } - (*mesh)["vertices"] = vertices; + Tmw = Tmw * node_it->mTransformation; + } + + if(node_it == nullptr) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "Could not find path '" << name << "' in input mesh file"); + // early stop + return mesh; + } + + if(node_it->mNumMeshes == 0) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "'" << name << "' of input mesh file has not meshes!"); + // early stop + return mesh; + } + + const aiMesh* amesh = getMeshByName(node_it, ascene->mMeshes, path_to_mesh.back()); + + if(amesh == nullptr) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "Leaf does not exist in mesh file!"); + return mesh; + } + + aiVector3D smw; + aiQuaternion Rmw; + aiVector3D tmw; + Tmw.Decompose(smw, Rmw, tmw); + + // fill this + mesh = std::make_shared(); - lvr2::Channel face_indices(amesh->mNumFaces, 3); - for(size_t i=0; imNumFaces; i++) - { - face_indices[i][0] = amesh->mFaces[i].mIndices[0]; - face_indices[i][1] = amesh->mFaces[i].mIndices[1]; - face_indices[i][2] = amesh->mFaces[i].mIndices[2]; - } - (*mesh)["face_indices"] = face_indices; - - if(amesh->HasNormals()) + lvr2::Channel vertices(amesh->mNumVertices, 3); + for(size_t i=0; imNumVertices; i++) + { + aiVector3D avertex = Tmw * amesh->mVertices[i]; + vertices[i][0] = avertex.x; + vertices[i][1] = avertex.y; + vertices[i][2] = avertex.z; + } + (*mesh)["vertices"] = vertices; + + lvr2::Channel face_indices(amesh->mNumFaces, 3); + for(size_t i=0; imNumFaces; i++) + { + face_indices[i][0] = amesh->mFaces[i].mIndices[0]; + face_indices[i][1] = amesh->mFaces[i].mIndices[1]; + face_indices[i][2] = amesh->mFaces[i].mIndices[2]; + } + (*mesh)["face_indices"] = face_indices; + + if(amesh->HasNormals()) + { + // vertex normals + lvr2::Channel vertex_normals(amesh->mNumVertices, 3); + for(size_t i=0; imNumVertices; i++) { - // vertex normals - lvr2::Channel vertex_normals(amesh->mNumVertices, 3); - for(size_t i=0; imNumVertices; i++) - { - vertex_normals[i][0] = amesh->mNormals[i].x; - vertex_normals[i][1] = amesh->mNormals[i].y; - vertex_normals[i][2] = amesh->mNormals[i].z; - } - (*mesh)["vertex_normals"] = vertex_normals; + aiVector3D anormal = Rmw.Rotate(amesh->mNormals[i]); + vertex_normals[i][0] = anormal.x; + vertex_normals[i][1] = anormal.y; + vertex_normals[i][2] = anormal.z; } + (*mesh)["vertex_normals"] = vertex_normals; + } - if(amesh->HasVertexColors(0)) + if(amesh->HasVertexColors(0)) + { + lvr2::Channel vertex_colors(amesh->mNumVertices, 4); + for(size_t i=0; imNumVertices; i++) { - lvr2::Channel vertex_colors(amesh->mNumVertices, 4); - for(size_t i=0; imNumVertices; i++) - { - vertex_colors[i][0] = amesh->mColors[0][i].r; - vertex_colors[i][1] = amesh->mColors[0][i].g; - vertex_colors[i][2] = amesh->mColors[0][i].b; - vertex_colors[i][3] = amesh->mColors[0][i].a; - } - (*mesh)["vertex_colors"] = vertex_colors; + vertex_colors[i][0] = amesh->mColors[0][i].r; + vertex_colors[i][1] = amesh->mColors[0][i].g; + vertex_colors[i][2] = amesh->mColors[0][i].b; + vertex_colors[i][3] = amesh->mColors[0][i].a; } + (*mesh)["vertex_colors"] = vertex_colors; } + return mesh; } From 544bbbaa3901c3e1faed3d8dd723875bdea447f7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 19:27:02 +0100 Subject: [PATCH 06/17] loading PLY works. DAE cannot be loaded still don't know why. --- mesh_layers/src/border_layer.cpp | 40 ++++++++++++++++------ mesh_layers/src/height_diff_layer.cpp | 16 +++++---- mesh_layers/src/inflation_layer.cpp | 21 ++++++++---- mesh_layers/src/ridge_layer.cpp | 9 +++-- mesh_layers/src/roughness_layer.cpp | 9 +++-- mesh_layers/src/steepness_layer.cpp | 6 ++-- mesh_map/include/mesh_map/abstract_layer.h | 10 ++++-- mesh_map/src/mesh_map.cpp | 22 +++++++++--- mesh_map/src/util.cpp | 16 +++++---- 9 files changed, 103 insertions(+), 46 deletions(-) diff --git a/mesh_layers/src/border_layer.cpp b/mesh_layers/src/border_layer.cpp index a4441655..9d2bbbe8 100644 --- a/mesh_layers/src/border_layer.cpp +++ b/mesh_layers/src/border_layer.cpp @@ -48,13 +48,12 @@ namespace mesh_layers bool BorderLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read border costs from map file..."); - auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap>("border"); + auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap >("border"); if (border_costs_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Border costs have been read successfully."); border_costs_ = border_costs_opt.get(); - return computeLethals(); } @@ -113,21 +112,39 @@ rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::v rcl_interfaces::msg::SetParametersResult result; result.successful = true; - bool has_threshold_changed = false; - for (auto parameter : parameters) { + // bool has_threshold_changed = false; + bool recompute_costs = false; + bool recompute_lethals = false; + + for (auto parameter : parameters) + { if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { config_.threshold = parameter.as_double(); - has_threshold_changed = true; + recompute_lethals = true; } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost") { config_.border_cost = parameter.as_double(); + recompute_costs = true; + recompute_lethals = true; } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { config_.factor = parameter.as_double(); } } - if (has_threshold_changed) { - RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change."); + if(recompute_costs) + { + RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute layer costs due to cfg change."); + computeLayer(); + } + + if (recompute_lethals) + { + RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute lethals due to cfg change."); computeLethals(); + } + + if(recompute_costs || recompute_lethals) + { + RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Notify changes."); notifyChange(); } @@ -140,29 +157,32 @@ bool BorderLayer::initialize() { // threshold rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Threshold for the local border costs to be counted as lethal."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.05; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold); + config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); } { // border cost rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The cost value used for the border vertices."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.02; range.to_value = 10.0; descriptor.floating_point_range.push_back(range); - config_.border_cost = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost", config_.border_cost); + config_.border_cost = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost", config_.border_cost, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Using this factor to weight this layer."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( &BorderLayer::reconfigureCallback, this, std::placeholders::_1)); diff --git a/mesh_layers/src/height_diff_layer.cpp b/mesh_layers/src/height_diff_layer.cpp index 7bb77f55..fb9b0157 100644 --- a/mesh_layers/src/height_diff_layer.cpp +++ b/mesh_layers/src/height_diff_layer.cpp @@ -111,19 +111,20 @@ rcl_interfaces::msg::SetParametersResult HeightDiffLayer::reconfigureCallback(st rcl_interfaces::msg::SetParametersResult result; result.successful = true; - bool has_threshold_changed = false; + bool recompute_lethals = false; for (auto parameter : parameters) { if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { config_.threshold = parameter.as_double(); - has_threshold_changed = true; + recompute_lethals = true; } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") { config_.radius = parameter.as_double(); + recompute_lethals = true; } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { config_.factor = parameter.as_double(); } } - if (has_threshold_changed) { + if (recompute_lethals) { RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change."); computeLethals(); notifyChange(); @@ -138,29 +139,32 @@ bool HeightDiffLayer::initialize() { // threshold rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Threshold for the local height difference to be counted as lethal."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.05; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold); + config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); } { // radius rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The radius used for calculating the local height difference."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.02; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius); + config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Using this factor to weight this layer."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( &HeightDiffLayer::reconfigureCallback, this, std::placeholders::_1)); diff --git a/mesh_layers/src/inflation_layer.cpp b/mesh_layers/src/inflation_layer.cpp index 4d26b782..c7c3c92f 100644 --- a/mesh_layers/src/inflation_layer.cpp +++ b/mesh_layers/src/inflation_layer.cpp @@ -678,61 +678,68 @@ bool InflationLayer::initialize() { // inscribed_radius rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the inscribed radius."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.inscribed_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius", config_.inscribed_radius); + config_.inscribed_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius", config_.inscribed_radius, descriptor); } { // inflation_radius rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the maximum inflation radius."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 3.0; descriptor.floating_point_range.push_back(range); - config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius); + config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The factor to weight this layer"; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.-0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } { // lethal_value rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the 'lethal' value for obstacles. -1 results in infinity"; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = -1.0; range.to_value = 100000.0; descriptor.floating_point_range.push_back(range); - config_.lethal_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".lethal_value", config_.lethal_value); + config_.lethal_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".lethal_value", config_.lethal_value, descriptor); } { // inscribed_value rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the 'inscribed' value for obstacles."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 100000.0; descriptor.floating_point_range.push_back(range); - config_.inscribed_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value", config_.inscribed_value); + config_.inscribed_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value", config_.inscribed_value, descriptor); } { // min_contour_size rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the minimum size for a contour to be classified as 'lethal'."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; rcl_interfaces::msg::IntegerRange range; range.from_value = 0; range.to_value = 100000; descriptor.integer_range.push_back(range); - config_.min_contour_size = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".min_contour_size", config_.min_contour_size); + config_.min_contour_size = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".min_contour_size", config_.min_contour_size, descriptor); } { // repulsive_field rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Enable the repulsive vector field."; - config_.repulsive_field = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".repulsive_field", config_.repulsive_field); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + config_.repulsive_field = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".repulsive_field", config_.repulsive_field, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(&InflationLayer::reconfigureCallback, this, std::placeholders::_1)); return true; diff --git a/mesh_layers/src/ridge_layer.cpp b/mesh_layers/src/ridge_layer.cpp index 0f24efb1..a1958071 100644 --- a/mesh_layers/src/ridge_layer.cpp +++ b/mesh_layers/src/ridge_layer.cpp @@ -219,29 +219,32 @@ bool RidgeLayer::initialize() { // threshold rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Threshold for the local ridge to be counted as lethal."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 3.1415; descriptor.floating_point_range.push_back(range); - config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold); + config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); } { // radius rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Radius of the inscribed area."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius); + config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The local ridge factor to weight this layer."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( &RidgeLayer::reconfigureCallback, this, std::placeholders::_1)); diff --git a/mesh_layers/src/roughness_layer.cpp b/mesh_layers/src/roughness_layer.cpp index 0160c169..1e567a71 100644 --- a/mesh_layers/src/roughness_layer.cpp +++ b/mesh_layers/src/roughness_layer.cpp @@ -169,29 +169,32 @@ bool RoughnessLayer::initialize() { { // threshold rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Threshold for the local roughness to be counted as lethal."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 3.1415; descriptor.floating_point_range.push_back(range); - config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold); + config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); } { // radius rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The radius used for calculating the local roughness."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.02; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius); + config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The local roughness factor to weight this layer."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( &RoughnessLayer::reconfigureCallback, this, std::placeholders::_1)); diff --git a/mesh_layers/src/steepness_layer.cpp b/mesh_layers/src/steepness_layer.cpp index 3278b546..e0fbfdb5 100644 --- a/mesh_layers/src/steepness_layer.cpp +++ b/mesh_layers/src/steepness_layer.cpp @@ -194,20 +194,22 @@ bool SteepnessLayer::initialize() { // threshold rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Threshold for the local steepness to be counted as lethal."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.01; range.to_value = 3.1415; descriptor.floating_point_range.push_back(range); - config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold); + config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); } { // factor rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "The local steepness factor to weight this layer."; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor); + config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); } dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( &SteepnessLayer::reconfigureCallback, this, std::placeholders::_1)); diff --git a/mesh_map/include/mesh_map/abstract_layer.h b/mesh_map/include/mesh_map/abstract_layer.h index 99216dc9..8ae0f39d 100644 --- a/mesh_map/include/mesh_map/abstract_layer.h +++ b/mesh_map/include/mesh_map/abstract_layer.h @@ -156,9 +156,13 @@ class AbstractLayer /** * @brief Initializes the layer plugin under the mesh_map namespace ans sets some basic attributes. */ - virtual bool initialize(const std::string& name, const notify_func notify_update, - std::shared_ptr& map, std::shared_ptr>& mesh, - std::shared_ptr& io, const rclcpp::Node::SharedPtr& node) + virtual bool initialize( + const std::string& name, + const notify_func notify_update, + std::shared_ptr& map, + std::shared_ptr>& mesh, + std::shared_ptr& io, + const rclcpp::Node::SharedPtr& node) { layer_name_ = name; node_ = node; diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 14288d1e..e0b042d8 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -177,7 +177,6 @@ bool MeshMap::readMap() } // directly work on the input file - RCLCPP_INFO_STREAM(node->get_logger(), "Connect to \"" << mesh_working_part << "\" from file \"" << mesh_working_file << "\"..."); auto hdf5_mesh_io = std::make_shared(); @@ -202,9 +201,16 @@ bool MeshMap::readMap() // TODO: load all attributes? } else { // use another loader + + // use assimp Assimp::Importer io; io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); - const aiScene* ascene = io.ReadFile(mesh_file, 0); + const aiScene* ascene = io.ReadFile(mesh_file, + aiProcess_Triangulate | + aiProcess_JoinIdenticalVertices | + aiProcess_GenNormals | + aiProcess_ValidateDataStructure | + aiProcess_FindInvalidData); if (!ascene) { RCLCPP_ERROR_STREAM(node->get_logger(), "Error while loading map: " << io.GetErrorString()); @@ -215,7 +221,7 @@ bool MeshMap::readMap() if(!mesh_buffer) { - RCLCPP_ERROR_STREAM(node->get_logger(), "Couldn't load mesh part: " << mesh_part); + RCLCPP_ERROR_STREAM(node->get_logger(), "Couldn't load mesh part: '" << mesh_part << "'"); } // write @@ -262,7 +268,6 @@ bool MeshMap::readMap() uuid_str = boost::uuids::to_string(uuid); auto face_normals_opt = mesh_io_ptr->getDenseAttributeMap>("face_normals"); - if (face_normals_opt) { face_normals = face_normals_opt.get(); @@ -346,6 +351,7 @@ bool MeshMap::readMap() return false; } + // why? sleep(1); combineVertexCosts(map_stamp); @@ -450,12 +456,18 @@ bool MeshMap::initLayerPlugins() layer_plugin->updateLethal(lethals, empty); if (!layer_plugin->readLayer()) { + RCLCPP_INFO_STREAM(node->get_logger(), "Computing layer '" << layer_name << "' ..."); layer_plugin->computeLayer(); + + // RCLCPP_INFO_STREAM(node->get_logger(), "Writing '" << layer_name << "' to file."); + // layer_plugin->writeLayer(); + // RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file."); } lethal_indices[layer_name].insert(layer_plugin->lethals().begin(), layer_plugin->lethals().end()); lethals.insert(layer_plugin->lethals().begin(), layer_plugin->lethals().end()); } + return true; } @@ -475,7 +487,7 @@ void MeshMap::combineVertexCosts(const rclcpp::Time& map_stamp) float min, max; mesh_map::getMinMax(costs, min, max); const float norm = max - min; - const float factor = 1.0; + const float factor = 1.0; // TODO how to declare param for each plugin? // Needs to be done after plugins are loaded, which happens when the map gets loaded. Who calls readMap() and when? // const float factor = private_nh.param(MESH_MAP_NAMESPACE + "/" + layer.first + "/factor", 1.0); diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index de03957f..53c7e9bf 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -102,14 +102,8 @@ lvr2::MeshBufferPtr extractMeshByName( { lvr2::MeshBufferPtr mesh; - const aiNode* root_node = ascene->mRootNode; - - // ascene->mMe - - // const aiNode* mesh_part_node = extractNodeByName(root_node, name); - // bool path_existing = false; // transform from mesh to world aiMatrix4x4 Tmw; @@ -176,8 +170,17 @@ lvr2::MeshBufferPtr extractMeshByName( (*mesh)["vertices"] = vertices; lvr2::Channel face_indices(amesh->mNumFaces, 3); + if(amesh->mNumFaces == 0) + { + throw std::runtime_error("TRIED TO LOAD 0 TRIANGLES"); + } for(size_t i=0; imNumFaces; i++) { + if(amesh->mFaces[i].mNumIndices != 3) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("mesh_map/util"), "Mesh contains elements that are no triangles: " << amesh->mFaces[i].mNumIndices); + throw std::runtime_error("TRIED TO LOAD NON-TRIANGLES"); + } face_indices[i][0] = amesh->mFaces[i].mIndices[0]; face_indices[i][1] = amesh->mFaces[i].mIndices[1]; face_indices[i][2] = amesh->mFaces[i].mIndices[2]; @@ -210,7 +213,6 @@ lvr2::MeshBufferPtr extractMeshByName( } (*mesh)["vertex_colors"] = vertex_colors; } - return mesh; } From fa39eb4099be0d0e2971087061216771813dee59 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 19:41:45 +0100 Subject: [PATCH 07/17] added service to save the changed layers --- mesh_layers/CMakeLists.txt | 2 +- mesh_map/CMakeLists.txt | 1 + mesh_map/include/mesh_map/mesh_map.h | 9 +++++++++ mesh_map/package.xml | 1 + mesh_map/src/mesh_map.cpp | 20 ++++++++++++++++++++ 5 files changed, 32 insertions(+), 1 deletion(-) diff --git a/mesh_layers/CMakeLists.txt b/mesh_layers/CMakeLists.txt index 1b502bdb..666c7907 100644 --- a/mesh_layers/CMakeLists.txt +++ b/mesh_layers/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(mesh_layers) -find_package(ament_cmake_ros COMPONENTS) +find_package(ament_cmake_ros REQUIRED) find_package(mesh_map) find_package(LVR2) diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index 3b6f8224..86514813 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -13,6 +13,7 @@ set(dependencies tf2_geometry_msgs tf2_ros visualization_msgs + std_srvs ) foreach(dep IN LISTS dependencies) find_package(${dep} REQUIRED) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 7b6d9d67..5f1d90d9 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -53,6 +53,8 @@ #include #include #include +#include + #include "nanoflann.hpp" #include "nanoflann_mesh_adaptor.h" @@ -398,6 +400,11 @@ class MeshMap */ mesh_map::AbstractLayer::Ptr layer(const std::string& layer_name); + /** + * @brief calls 'saveLayer' on every active layer + */ + void saveLayers(); + //! This is an abstract interface to load mesh information from somewhere //! The most default case is loading from a HDF5 file //! However we could also implement a server connection here @@ -442,6 +449,8 @@ class MeshMap //! dynamic params callback handle rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr config_callback; + rclcpp::Service::SharedPtr save_service; + // Reconfigurable parameters (see reconfigureCallback method) int min_contour_size; double layer_factor; diff --git a/mesh_map/package.xml b/mesh_map/package.xml index d2978ae9..f1c4864e 100644 --- a/mesh_map/package.xml +++ b/mesh_map/package.xml @@ -17,6 +17,7 @@ tf2_ros tf2 visualization_msgs + std_srvs assimp ament_cmake_gmock diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index e0b042d8..daf2e9f3 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -147,6 +147,13 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) vertex_colors_pub = node->create_publisher("~/vertex_colors", rclcpp::QoS(1).transient_local()); vector_field_pub = node->create_publisher("~/vector_field", rclcpp::QoS(1).transient_local()); config_callback = node->add_on_set_parameters_callback(std::bind(&MeshMap::reconfigureCallback, this, std::placeholders::_1)); + + save_service = node->create_service("~/save_map", [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + saveLayers(); + }); } bool MeshMap::readMap() @@ -1164,6 +1171,19 @@ mesh_map::AbstractLayer::Ptr MeshMap::layer(const std::string& layer_name) })->second; } +void MeshMap::saveLayers() +{ + for (auto& layer : loaded_layers) + { + auto& layer_plugin = layer.second; + const auto& layer_name = layer.first; + + RCLCPP_INFO_STREAM(node->get_logger(), "Writing '" << layer_name << "' to file."); + layer_plugin->writeLayer(); + RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file."); + } +} + bool MeshMap::barycentricCoords(const Vector& p, const lvr2::FaceHandle& triangle, float& u, float& v, float& w) { const auto& face = mesh_ptr->getVertexPositionsOfFace(triangle); From ec6721f62c57b2228e21f0bf4d4e03481e777997 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Nov 2024 20:00:54 +0100 Subject: [PATCH 08/17] layer are pointing to same named hdf5 datasets --- mesh_layers/include/mesh_layers/border_layer.h | 3 +++ mesh_layers/src/border_layer.cpp | 5 ++--- mesh_layers/src/height_diff_layer.cpp | 4 ++-- mesh_layers/src/inflation_layer.cpp | 4 ++-- mesh_layers/src/ridge_layer.cpp | 4 ++-- mesh_layers/src/roughness_layer.cpp | 4 ++-- mesh_layers/src/steepness_layer.cpp | 4 ++-- mesh_map/src/mesh_map.cpp | 6 +++--- 8 files changed, 18 insertions(+), 16 deletions(-) diff --git a/mesh_layers/include/mesh_layers/border_layer.h b/mesh_layers/include/mesh_layers/border_layer.h index 18843834..ed724e7f 100644 --- a/mesh_layers/include/mesh_layers/border_layer.h +++ b/mesh_layers/include/mesh_layers/border_layer.h @@ -141,6 +141,9 @@ class BorderLayer : public mesh_map::AbstractLayer double border_cost = 1.0; double factor = 1.0; } config_; + + // put this to base class? + std::string name; }; } /* namespace mesh_layers */ diff --git a/mesh_layers/src/border_layer.cpp b/mesh_layers/src/border_layer.cpp index 9d2bbbe8..b420a415 100644 --- a/mesh_layers/src/border_layer.cpp +++ b/mesh_layers/src/border_layer.cpp @@ -48,7 +48,7 @@ namespace mesh_layers bool BorderLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read border costs from map file..."); - auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap >("border"); + auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap >(layer_name_); if (border_costs_opt) { @@ -79,7 +79,7 @@ bool BorderLayer::computeLethals() bool BorderLayer::writeLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Saving border costs to map file..."); - if (mesh_io_ptr_->addDenseAttributeMap(border_costs_, "border")) + if (mesh_io_ptr_->addDenseAttributeMap(border_costs_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved border costs to map file."); return true; @@ -151,7 +151,6 @@ rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::v return result; } - bool BorderLayer::initialize() { { // threshold diff --git a/mesh_layers/src/height_diff_layer.cpp b/mesh_layers/src/height_diff_layer.cpp index fb9b0157..f0c2c90d 100644 --- a/mesh_layers/src/height_diff_layer.cpp +++ b/mesh_layers/src/height_diff_layer.cpp @@ -48,7 +48,7 @@ namespace mesh_layers bool HeightDiffLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read height differences from map file..."); - auto height_diff_opt = mesh_io_ptr_->getDenseAttributeMap>("height_diff"); + auto height_diff_opt = mesh_io_ptr_->getDenseAttributeMap>(layer_name_); if (height_diff_opt) { @@ -78,7 +78,7 @@ bool HeightDiffLayer::computeLethals() bool HeightDiffLayer::writeLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Saving height_differences to map file..."); - if (mesh_io_ptr_->addDenseAttributeMap(height_diff_, "height_diff")) + if (mesh_io_ptr_->addDenseAttributeMap(height_diff_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved height differences to map file."); return true; diff --git a/mesh_layers/src/inflation_layer.cpp b/mesh_layers/src/inflation_layer.cpp index c7c3c92f..35635e0a 100644 --- a/mesh_layers/src/inflation_layer.cpp +++ b/mesh_layers/src/inflation_layer.cpp @@ -50,7 +50,7 @@ bool InflationLayer::readLayer() { // riskiness RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read riskiness from map file..."); - auto riskiness_opt = mesh_io_ptr_->getDenseAttributeMap>("riskiness"); + auto riskiness_opt = mesh_io_ptr_->getDenseAttributeMap>(layer_name_); if (riskiness_opt) { @@ -68,7 +68,7 @@ bool InflationLayer::writeLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Saving " << riskiness_.numValues() << " riskiness values to map file..."); - if (mesh_io_ptr_->addDenseAttributeMap(riskiness_, "riskiness")) + if (mesh_io_ptr_->addDenseAttributeMap(riskiness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved riskiness to map file."); return true; diff --git a/mesh_layers/src/ridge_layer.cpp b/mesh_layers/src/ridge_layer.cpp index a1958071..7e930eb9 100644 --- a/mesh_layers/src/ridge_layer.cpp +++ b/mesh_layers/src/ridge_layer.cpp @@ -50,7 +50,7 @@ namespace mesh_layers bool RidgeLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read ridge from map file..."); - auto ridge_opt = mesh_io_ptr_->getDenseAttributeMap>("ridge"); + auto ridge_opt = mesh_io_ptr_->getDenseAttributeMap>(layer_name_); if (ridge_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read ridge from map file."); @@ -63,7 +63,7 @@ bool RidgeLayer::readLayer() bool RidgeLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(ridge_, "ridge")) + if (mesh_io_ptr_->addDenseAttributeMap(ridge_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved ridge to map file."); return true; diff --git a/mesh_layers/src/roughness_layer.cpp b/mesh_layers/src/roughness_layer.cpp index 1e567a71..b6f31b71 100644 --- a/mesh_layers/src/roughness_layer.cpp +++ b/mesh_layers/src/roughness_layer.cpp @@ -49,7 +49,7 @@ bool RoughnessLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read roughness from map file..."); auto roughness_opt = mesh_io_ptr_->getDenseAttributeMap>( - "roughness"); + layer_name_); if (roughness_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read roughness from map file."); roughness_ = roughness_opt.get(); @@ -60,7 +60,7 @@ bool RoughnessLayer::readLayer() { } bool RoughnessLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(roughness_, "roughness")) { + if (mesh_io_ptr_->addDenseAttributeMap(roughness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved roughness to map file."); return true; } else { diff --git a/mesh_layers/src/steepness_layer.cpp b/mesh_layers/src/steepness_layer.cpp index e0fbfdb5..65be9764 100644 --- a/mesh_layers/src/steepness_layer.cpp +++ b/mesh_layers/src/steepness_layer.cpp @@ -49,7 +49,7 @@ namespace mesh_layers bool SteepnessLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read steepness from map file..."); - auto steepness_opt = mesh_io_ptr_->getDenseAttributeMap>("steepness"); + auto steepness_opt = mesh_io_ptr_->getDenseAttributeMap>(layer_name_); if (steepness_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read steepness from map file."); @@ -62,7 +62,7 @@ bool SteepnessLayer::readLayer() bool SteepnessLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(steepness_, "steepness")) + if (mesh_io_ptr_->addDenseAttributeMap(steepness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved steepness to map file."); return true; diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index daf2e9f3..2684dcab 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -162,15 +162,15 @@ bool MeshMap::readMap() { if(!mesh_file.empty() && !mesh_part.empty()) { - // default: mesh_working_file = mesh_file + if(mesh_working_file == "") { - mesh_working_file = fs::path(mesh_file).replace_extension(".h5"); + // default: mesh_working_file = mesh_file filename in this directory + mesh_working_file = fs::path(mesh_file).filename().replace_extension(".h5"); } if(mesh_working_part == "") { - std::cout << "BLABLAL: " << mesh_part << std::endl; mesh_working_part = mesh_part; RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part es default: '" << mesh_working_part << "'"); } else { From 1c9a67e2686982427e6b15cef913088f7b3fbd3e Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:21:59 +0100 Subject: [PATCH 09/17] Update mesh_map/include/mesh_map/mesh_map.h Co-authored-by: Matthias Holoch --- mesh_map/include/mesh_map/mesh_map.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 5f1d90d9..fce005dc 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -406,7 +406,7 @@ class MeshMap void saveLayers(); //! This is an abstract interface to load mesh information from somewhere - //! The most default case is loading from a HDF5 file + //! The default case is loading from a HDF5 file //! However we could also implement a server connection here //! We might use the pluginlib for that std::shared_ptr mesh_io_ptr; From 07ca2748d1eb95b27c69bfbb3eda17ba07df1cb7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:23:34 +0100 Subject: [PATCH 10/17] Update mesh_map/src/mesh_map.cpp Co-authored-by: Matthias Holoch --- mesh_map/src/mesh_map.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 2684dcab..9a7edf71 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -172,7 +172,7 @@ bool MeshMap::readMap() if(mesh_working_part == "") { mesh_working_part = mesh_part; - RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part es default: '" << mesh_working_part << "'"); + RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part as default: '" << mesh_working_part << "'"); } else { RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is *not* empty: '" << mesh_working_part << "'"); } From acb2c0f9bf257a8302c2c85e3a249f5c972c3856 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:27:21 +0100 Subject: [PATCH 11/17] Update mesh_map/src/mesh_map.cpp Co-authored-by: Matthias Holoch --- mesh_map/src/mesh_map.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 9a7edf71..bc346c5e 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -174,7 +174,7 @@ bool MeshMap::readMap() mesh_working_part = mesh_part; RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part as default: '" << mesh_working_part << "'"); } else { - RCLCPP_INFO_STREAM(node->get_logger(), "Mesh Working Part is *not* empty: '" << mesh_working_part << "'"); + RCLCPP_INFO_STREAM(node->get_logger(), "Using mesh working part from parameter: '" << mesh_working_part << "'"); } if(fs::path(mesh_working_file).extension() != ".h5") From c983453db1e5bfda7b5563aec3850e22eeaf2e8c Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:46:03 +0100 Subject: [PATCH 12/17] cleanup --- mesh_map/include/mesh_map/mesh_map.h | 14 ++++++++++---- mesh_map/src/mesh_map.cpp | 11 +---------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index fce005dc..84c1c1d3 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -434,16 +434,22 @@ class MeshMap //! global frame / coordinate system id std::string global_frame; - //! mesh file + //! Filename of the input mesh. Can be any format the assimp library supports to load. + //! https://github.com/assimp/assimp/blob/master/doc/Fileformats.md std::string mesh_file; - //! mesh part + //! Part of the mesh that is loaded for navigation + //! For HDF5 meshes this is the H5 group name of the desired mesh part + //! For standard formats this would be the internal path thorugh the scene graph to the mesh std::string mesh_part; - //! mesh working file + //! This is the filename of the file where the work is done. Choosing it differently from the + //! input mesh prevents from accidentially overwrite your input data. + //! By default it will be set to the input mesh filename. + //! The working file must be of format HDF5 std::string mesh_working_file; - //! mesh working part + //! The name of the mesh part that is used for navigation stored to the working file std::string mesh_working_part; //! dynamic params callback handle diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index bc346c5e..2dfcb196 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -358,9 +358,6 @@ bool MeshMap::readMap() return false; } - // why? - sleep(1); - combineVertexCosts(map_stamp); publishCostLayers(map_stamp); @@ -465,10 +462,6 @@ bool MeshMap::initLayerPlugins() { RCLCPP_INFO_STREAM(node->get_logger(), "Computing layer '" << layer_name << "' ..."); layer_plugin->computeLayer(); - - // RCLCPP_INFO_STREAM(node->get_logger(), "Writing '" << layer_name << "' to file."); - // layer_plugin->writeLayer(); - // RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file."); } lethal_indices[layer_name].insert(layer_plugin->lethals().begin(), layer_plugin->lethals().end()); @@ -495,9 +488,7 @@ void MeshMap::combineVertexCosts(const rclcpp::Time& map_stamp) mesh_map::getMinMax(costs, min, max); const float norm = max - min; const float factor = 1.0; - // TODO how to declare param for each plugin? - // Needs to be done after plugins are loaded, which happens when the map gets loaded. Who calls readMap() and when? - // const float factor = private_nh.param(MESH_MAP_NAMESPACE + "/" + layer.first + "/factor", 1.0); + // TODO: carefully think about this const float norm_factor = factor / norm; RCLCPP_INFO_STREAM(node->get_logger(), "Layer \"" << layer.first << "\" max value: " << max << " min value: " << min << " norm: " << norm << " factor: " << factor << " norm factor: " << norm_factor); From 0787ac1a83c4a1f66c7d950539725ae7c89eb68e Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:49:05 +0100 Subject: [PATCH 13/17] Update mesh_map/src/util.cpp Co-authored-by: Matthias Holoch --- mesh_map/src/util.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index 53c7e9bf..c5b2587f 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -104,7 +104,6 @@ lvr2::MeshBufferPtr extractMeshByName( const aiNode* root_node = ascene->mRootNode; - // bool path_existing = false; // transform from mesh to world aiMatrix4x4 Tmw; From c087a3350ced64f78d2bab305e7bbae4edecf16c Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 11:55:24 +0100 Subject: [PATCH 14/17] removed name member variable since it is not used anymore --- mesh_layers/include/mesh_layers/border_layer.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/mesh_layers/include/mesh_layers/border_layer.h b/mesh_layers/include/mesh_layers/border_layer.h index ed724e7f..18843834 100644 --- a/mesh_layers/include/mesh_layers/border_layer.h +++ b/mesh_layers/include/mesh_layers/border_layer.h @@ -141,9 +141,6 @@ class BorderLayer : public mesh_map::AbstractLayer double border_cost = 1.0; double factor = 1.0; } config_; - - // put this to base class? - std::string name; }; } /* namespace mesh_layers */ From 2b95a069bb810458a4fe00f7c91dbd0ed796a0fe Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 19 Nov 2024 12:02:48 +0100 Subject: [PATCH 15/17] added docs to saveLayers --- mesh_map/include/mesh_map/mesh_map.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 84c1c1d3..bae8915c 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -401,7 +401,14 @@ class MeshMap mesh_map::AbstractLayer::Ptr layer(const std::string& layer_name); /** - * @brief calls 'saveLayer' on every active layer + * @brief calls 'saveLayer' on every active layer. Every layer itself writes its costs + * to the working file / part to a dataset named after the layer-name. + * + * Example: + * - Working file: "my_map.h5" + * - Working mesh part: "my_mesh_part" + * + * A BorderLayer of name 'border' would write the costs to "my_map.h5/my_mesh_part/channels/border" */ void saveLayers(); From 852481951884b36af14598fddfac0b6c6a060e1d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 20 Nov 2024 00:10:07 +0100 Subject: [PATCH 16/17] added std_srvs Trigger instead of Empty --- mesh_map/include/mesh_map/mesh_map.h | 11 ++++++---- mesh_map/src/mesh_map.cpp | 32 +++++++++++++++++++++------- 2 files changed, 31 insertions(+), 12 deletions(-) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index bae8915c..5f0a9880 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include "nanoflann.hpp" #include "nanoflann_mesh_adaptor.h" @@ -401,7 +401,7 @@ class MeshMap mesh_map::AbstractLayer::Ptr layer(const std::string& layer_name); /** - * @brief calls 'saveLayer' on every active layer. Every layer itself writes its costs + * @brief calls 'writeLayer' on every active layer. Every layer itself writes its costs * to the working file / part to a dataset named after the layer-name. * * Example: @@ -409,8 +409,11 @@ class MeshMap * - Working mesh part: "my_mesh_part" * * A BorderLayer of name 'border' would write the costs to "my_map.h5/my_mesh_part/channels/border" + * + * @return Trigger response. If success is false, the message field is + * beeing filled with all the failed layer names seperated by comma */ - void saveLayers(); + std_srvs::srv::Trigger::Response writeLayers(); //! This is an abstract interface to load mesh information from somewhere //! The default case is loading from a HDF5 file @@ -462,7 +465,7 @@ class MeshMap //! dynamic params callback handle rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr config_callback; - rclcpp::Service::SharedPtr save_service; + rclcpp::Service::SharedPtr save_service; // Reconfigurable parameters (see reconfigureCallback method) int min_contour_size; diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 2dfcb196..80c456b1 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -148,11 +148,11 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) vector_field_pub = node->create_publisher("~/vector_field", rclcpp::QoS(1).transient_local()); config_callback = node->add_on_set_parameters_callback(std::bind(&MeshMap::reconfigureCallback, this, std::placeholders::_1)); - save_service = node->create_service("~/save_map", [this]( - const std::shared_ptr request, - std::shared_ptr response) + save_service = node->create_service("~/save_map", [this]( + const std::shared_ptr request, + std::shared_ptr response) { - saveLayers(); + *response = writeLayers(); }); } @@ -253,7 +253,7 @@ bool MeshMap::readMap() RCLCPP_INFO_STREAM(node->get_logger(), "The mesh has been loaded successfully with " << mesh_ptr->numVertices() << " vertices and " << mesh_ptr->numFaces() << " faces and " << mesh_ptr->numEdges() << " edges."); - + // build a tree for fast lookups adaptor_ptr = std::make_unique(*mesh_ptr); kd_tree_ptr = std::make_unique(3,*adaptor_ptr, nanoflann::KDTreeSingleIndexAdaptorParams(10)); kd_tree_ptr->buildIndex(); @@ -1162,17 +1162,33 @@ mesh_map::AbstractLayer::Ptr MeshMap::layer(const std::string& layer_name) })->second; } -void MeshMap::saveLayers() +std_srvs::srv::Trigger::Response MeshMap::writeLayers() { + std::stringstream ss; + bool write_failure = false; + for (auto& layer : loaded_layers) { auto& layer_plugin = layer.second; const auto& layer_name = layer.first; RCLCPP_INFO_STREAM(node->get_logger(), "Writing '" << layer_name << "' to file."); - layer_plugin->writeLayer(); - RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file."); + if(layer_plugin->writeLayer()) + { + RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file."); + } else { + // this is not the first failure. add a comma in between + if(write_failure){ss << ",";} + ss << layer_name; + write_failure = true; + RCLCPP_ERROR_STREAM(node->get_logger(), "Error while writing '" << layer_name << "' to file."); + } } + + std_srvs::srv::Trigger::Response res; + res.success = !write_failure; + res.message = ss.str(); + return res; } bool MeshMap::barycentricCoords(const Vector& p, const lvr2::FaceHandle& triangle, float& u, float& v, float& w) From ef42e0a1c2079507ebaa44718ca7ecd03b50b1b9 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 20 Nov 2024 00:41:04 +0100 Subject: [PATCH 17/17] if only a h5 is provided the same file is used as input and working file --- mesh_map/src/mesh_map.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 80c456b1..9c0dcd3d 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -162,11 +162,10 @@ bool MeshMap::readMap() { if(!mesh_file.empty() && !mesh_part.empty()) { - if(mesh_working_file == "") { // default: mesh_working_file = mesh_file filename in this directory - mesh_working_file = fs::path(mesh_file).filename().replace_extension(".h5"); + mesh_working_file = fs::path(mesh_file).replace_extension(".h5"); } if(mesh_working_part == "")