From 5f5a1e9adb3578d751cc3508130f459c1cbaecb7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 5 Dec 2024 17:33:49 +0100 Subject: [PATCH 1/9] started to adapt to lvr update --- COLCON_IGNORE | 0 mesh_client/COLCON_IGNORE | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 COLCON_IGNORE create mode 100644 mesh_client/COLCON_IGNORE diff --git a/COLCON_IGNORE b/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/mesh_client/COLCON_IGNORE b/mesh_client/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b From bf6bba1e6decb09abd01a1cff4f41a4528c4b97b Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 6 Dec 2024 15:21:58 +0100 Subject: [PATCH 2/9] compiles with new version of lvr now --- COLCON_IGNORE | 0 cvp_mesh_planner/src/cvp_mesh_planner.cpp | 84 +++++++++---------- .../src/dijkstra_mesh_planner.cpp | 27 +++--- mesh_controller/src/mesh_controller.cpp | 4 +- mesh_layers/CMakeLists.txt | 16 +++- mesh_layers/src/border_layer.cpp | 8 +- mesh_layers/src/height_diff_layer.cpp | 9 +- mesh_layers/src/inflation_layer.cpp | 70 +++++++++------- mesh_layers/src/ridge_layer.cpp | 32 +++---- mesh_layers/src/roughness_layer.cpp | 23 +++-- mesh_layers/src/steepness_layer.cpp | 26 +++--- mesh_map/CMakeLists.txt | 4 +- mesh_map/include/mesh_map/abstract_layer.h | 10 +-- mesh_map/include/mesh_map/mesh_map.h | 28 +++++-- .../include/mesh_map/nanoflann_mesh_adaptor.h | 12 +-- mesh_map/include/mesh_map/util.h | 2 +- mesh_map/package.xml | 1 - mesh_map/src/mesh_map.cpp | 32 +++---- 18 files changed, 211 insertions(+), 177 deletions(-) delete mode 100644 COLCON_IGNORE diff --git a/COLCON_IGNORE b/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index fcbbebd6..8e259ade 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -65,7 +65,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, std::vector& plan, double& cost, std::string& message) { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); std::list> path; // mesh_map->combineVertexCosts(); // TODO should be outside the planner @@ -161,8 +161,8 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share } path_pub_ = node->create_publisher("~/path", rclcpp::QoS(1).transient_local()); - const auto& mesh = mesh_map_->mesh(); - direction_ = lvr2::DenseVertexMap(mesh.nextVertexIndex(), 0); + const auto mesh = mesh_map_->mesh(); + direction_ = lvr2::DenseVertexMap(mesh->nextVertexIndex(), 0); // TODO check all map dependencies! (loaded layers etc...) reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind( @@ -189,11 +189,11 @@ rcl_interfaces::msg::SetParametersResult CVPMeshPlanner::reconfigureCallback(std void CVPMeshPlanner::computeVectorMap() { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); const auto& face_normals = mesh_map_->faceNormals(); const auto& vertex_normals = mesh_map_->vertexNormals(); - for (auto v3 : mesh.vertices()) + for (auto v3 : mesh->vertices()) { // if(vertex_costs[v3] > config.cost_limit || !predecessors.containsKey(v3)) // continue; @@ -212,8 +212,8 @@ void CVPMeshPlanner::computeVectorMap() const lvr2::FaceHandle& fH = optFh.get(); - const auto& vec3 = mesh.getVertexPosition(v3); - const auto& vec1 = mesh.getVertexPosition(v1); + const auto& vec3 = mesh->getVertexPosition(v3); + const auto& vec1 = mesh->getVertexPosition(v1); // compute the direction vector and rotate it by theta, which is stored in // the direction vertex map @@ -237,21 +237,21 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3) { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); const double u1 = distances[v1]; const double u2 = distances[v2]; const double u3 = distances[v3]; - const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2); + const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2); const double c = edge_weights[e12h.unwrap()]; const double c_sq = c * c; - const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3); + const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3); const double b = edge_weights[e13h.unwrap()]; const double b_sq = b * b; - const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3); + const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3); const double a = edge_weights[e23h.unwrap()]; const double a_sq = a * a; @@ -284,7 +284,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di predecessors_[v3] = v1; direction_[v3] = static_cast(theta); distances[v3] = static_cast(u3tmp); - const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); + const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fh, theta, mesh_map::color(0.9, 0.9, 0.2), @@ -300,7 +300,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di predecessors_[v3] = v1; direction_[v3] = 0; distances[v3] = u3tmp; - const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); + const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fh, 0, mesh_map::color(0.9, 0.9, 0.2), @@ -317,7 +317,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di const double t2cos = (a_sq + u3tmp_sq - u2_sq) / (2 * a * u3tmp); if (S <= 0 && std::fabs(t2cos) <= 1) { - const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); + const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap(); const double theta = -acos(t2cos); direction_[v3] = static_cast(theta); distances[v3] = static_cast(u3tmp); @@ -337,7 +337,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di direction_[v3] = 0; distances[v3] = u3tmp; predecessors_[v3] = v2; - const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); + const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fh, 0, mesh_map::color(0.9, 0.9, 0.2), @@ -357,21 +357,21 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3) { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); const double u1 = distances[v1]; const double u2 = distances[v2]; const double u3 = distances[v3]; - const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2); + const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2); const double c = edge_weights[e12h.unwrap()]; const double c_sq = c * c; - const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3); + const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3); const double b = edge_weights[e13h.unwrap()]; const double b_sq = b * b; - const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3); + const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3); const double a = edge_weights[e23h.unwrap()]; const double a_sq = a * a; @@ -406,7 +406,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc u3tmp = u1 + b; if (u3tmp < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v1; #ifdef DEBUG @@ -425,7 +425,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc u3tmp = u2 + a; if (u3tmp < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v2; #ifdef DEBUG @@ -478,7 +478,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (theta1 < theta0 && theta2 < theta0) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); distances[v3] = static_cast(u3tmp); if (theta1 < theta2) @@ -506,7 +506,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc u3tmp = u1 + b; if (u3tmp < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v1; distances[v3] = static_cast(u3tmp); @@ -524,7 +524,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc u3tmp = u2 + a; if (u3tmp < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v2; distances[v3] = static_cast(u3tmp); @@ -549,7 +549,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( const lvr2::VertexHandle &v2tmp, const lvr2::VertexHandle &v3) { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); bool v1_smaller = distances[v1tmp] < distances[v2tmp]; const lvr2::VertexHandle v1 = v1_smaller ? v1tmp : v2tmp; @@ -559,15 +559,15 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( const double u2 = distances[v2]; const double u3 = distances[v3]; - const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2); + const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2); const double c = edge_weights[e12h.unwrap()]; const double c_sq = c * c; - const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3); + const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3); const double b = edge_weights[e13h.unwrap()]; const double b_sq = b * b; - const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3); + const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3); const double a = edge_weights[e23h.unwrap()]; const double a_sq = a * a; @@ -596,7 +596,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( const double u3_tmp = u1 + t; if(u3_tmp < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v1; distances[v3] = static_cast(u3_tmp); @@ -612,7 +612,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( if (u1t < u2t) { if (u1t < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v1; distances[v3] = static_cast(u1t); @@ -621,7 +621,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( } } else { if (u2t < u3) { - auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); + auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap(); cutting_faces_.insert(v3, fH); predecessors_[v3] = v2; distances[v3] = static_cast(u2t); @@ -645,7 +645,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s { RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init wave front propagation."); - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); const auto& vertex_costs = mesh_map_->vertexCosts(); auto& invalid = mesh_map_->invalid; @@ -683,14 +683,14 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s distances.clear(); predecessors.clear(); - lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); + lvr2::DenseVertexMap fixed(mesh->nextVertexIndex(), false); // clear vector field map vector_map_.clear(); // initialize distances with infinity // initialize predecessor of each vertex with itself - for (auto const& vH : mesh.vertices()) + for (auto const& vH : mesh->vertices()) { distances.insert(vH, std::numeric_limits::infinity()); predecessors.insert(vH, vH); @@ -699,9 +699,9 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s lvr2::Meap pq; // Set start distance to zero // add start vertex to priority queue - for (auto vH : mesh.getVerticesOfFace(start_face)) + for (auto vH : mesh->getVerticesOfFace(start_face)) { - const mesh_map::Vector diff = start - mesh.getVertexPosition(vH); + const mesh_map::Vector diff = start - mesh->getVertexPosition(vH); const float dist = diff.length(); distances[vH] = dist; vector_map_.insert(vH, diff); @@ -710,13 +710,13 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s pq.insert(vH, dist); } - std::array goal_vertices = mesh.getVerticesOfFace(goal_face); + std::array goal_vertices = mesh->getVerticesOfFace(goal_face); RCLCPP_DEBUG_STREAM(node_->get_logger(), "The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face (" << goal_vertices[0] << ", " << goal_vertices[1] << ", " << goal_vertices[2] << ")"); - mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1"); - mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2"); - mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3"); + mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1"); + mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2"); + mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3"); float goal_dist = std::numeric_limits::infinity(); @@ -756,11 +756,11 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s try { std::vector faces; - mesh.getFacesOfVertex(current_vh, faces); + mesh->getFacesOfVertex(current_vh, faces); for (auto fh : faces) { - const auto vertices = mesh.getVerticesOfFace(fh); + const auto vertices = mesh->getVerticesOfFace(fh); const lvr2::VertexHandle& a = vertices[0]; const lvr2::VertexHandle& b = vertices[1]; const lvr2::VertexHandle& c = vertices[2]; diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index 601663e1..1050def2 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -56,7 +56,8 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st double tolerance, std::vector& plan, double& cost, std::string& message) { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); + std::list path; RCLCPP_INFO(node_->get_logger(), "start dijkstra mesh planner."); @@ -87,7 +88,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st { // get next position const lvr2::VertexHandle& vH = path.front(); - mesh_map::Vector next = mesh.getVertexPosition(vH); + mesh_map::Vector next = mesh->getVertexPosition(vH); pose.pose = mesh_map::calculatePoseFromPosition(vec, next, normal, dir_length); cost += dir_length; @@ -146,7 +147,7 @@ bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std:: } path_pub_ = node_->create_publisher("~/path", rclcpp::QoS(1).transient_local()); - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind( &DijkstraMeshPlanner::reconfigureCallback, this, std::placeholders::_1)); @@ -174,17 +175,17 @@ rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallbac void DijkstraMeshPlanner::computeVectorMap() { - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); - for (auto v3 : mesh.vertices()) + for (auto v3 : mesh->vertices()) { const lvr2::VertexHandle& v1 = predecessors_[v3]; // if predecessor is pointing to it self, continue with the next vertex. if (v1 == v3) continue; - const auto& vec3 = mesh.getVertexPosition(v3); - const auto& vec1 = mesh.getVertexPosition(v1); + const auto& vec3 = mesh->getVertexPosition(v3); + const auto& vec1 = mesh->getVertexPosition(v1); // compute the direction vector and store it in the direction vertex map const auto dirVec = vec1 - vec3; @@ -209,7 +210,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c RCLCPP_INFO_STREAM(node_->get_logger(), "Init wave front propagation."); const auto t_initialization_start = std::chrono::steady_clock::now(); - const auto& mesh = mesh_map_->mesh(); + const auto mesh = mesh_map_->mesh(); const auto& vertex_costs = mesh_map_->vertexCosts(); auto& invalid = mesh_map_->invalid; @@ -240,7 +241,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c return mbf_msgs::action::GetPath::Result::SUCCESS; } - lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); + lvr2::DenseVertexMap fixed(mesh->nextVertexIndex(), false); // clear vector field map vector_map_.clear(); @@ -249,7 +250,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c // initialize distances with infinity // initialize predecessor of each vertex with itself - for (auto const& vH : mesh.vertices()) + for (auto const& vH : mesh->vertices()) { distances.insert(vH, std::numeric_limits::infinity()); predecessors.insert(vH, vH); @@ -291,7 +292,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c std::vector edges; try { - mesh.getEdgesOfVertex(current_vh, edges); + mesh->getEdgesOfVertex(current_vh, edges); } catch (lvr2::PanicException exception) { @@ -307,7 +308,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c { try { - std::array vertices = mesh.getVerticesOfEdge(eH); + std::array vertices = mesh->getVerticesOfEdge(eH); auto vH = vertices[0] == current_vh ? vertices[1] : vertices[0]; if (fixed[vH]) continue; @@ -361,7 +362,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c const auto t_end = std::chrono::steady_clock::now(); const auto execution_duration_ms = std::chrono::duration_cast(t_end - t_start); RCLCPP_INFO_STREAM(node_->get_logger(), "Execution duration (ms): " << execution_duration_ms.count() - << " for " << mesh.numVertices() << " num vertices in the mesh."); + << " for " << mesh->numVertices() << " num vertices in the mesh."); computeVectorMap(); diff --git a/mesh_controller/src/mesh_controller.cpp b/mesh_controller/src/mesh_controller.cpp index 5b8b2ba3..b2c86c88 100644 --- a/mesh_controller/src/mesh_controller.cpp +++ b/mesh_controller/src/mesh_controller.cpp @@ -69,7 +69,7 @@ uint32_t MeshController::computeVelocityCommands(const geometry_msgs::msg::PoseS geometry_msgs::msg::TwistStamped& cmd_vel, std::string& message) { - const auto& mesh = map_ptr_->mesh(); + const auto mesh = map_ptr_->mesh(); robot_pos_ = poseToPositionVector(pose); robot_dir_ = poseToDirectionVector(pose); @@ -99,7 +99,7 @@ uint32_t MeshController::computeVelocityCommands(const geometry_msgs::msg::PoseS else // current face is set { lvr2::FaceHandle face = current_face_.unwrap(); - vertices = mesh.getVertexPositionsOfFace(face); + vertices = mesh->getVertexPositionsOfFace(face); DEBUG_CALL(map_ptr_->publishDebugFace(face, mesh_map::color(1, 1, 1), "current_face");) DEBUG_CALL(map_ptr_->publishDebugPoint(robot_pos_, mesh_map::color(1, 1, 1), "robot_position");) diff --git a/mesh_layers/CMakeLists.txt b/mesh_layers/CMakeLists.txt index 666c7907..6c03ad63 100644 --- a/mesh_layers/CMakeLists.txt +++ b/mesh_layers/CMakeLists.txt @@ -2,8 +2,9 @@ cmake_minimum_required(VERSION 3.8) project(mesh_layers) find_package(ament_cmake_ros REQUIRED) -find_package(mesh_map) -find_package(LVR2) +find_package(mesh_map REQUIRED) +find_package(LVR2 REQUIRED) +add_definitions(${LVR2_DEFINITIONS}) pluginlib_export_plugin_description_file(mesh_map mesh_layers.xml) @@ -15,12 +16,21 @@ add_library(${PROJECT_NAME} src/steepness_layer.cpp src/ridge_layer.cpp ) +include_directories( + include + ${LVR2_INCLUDE_DIRS} +) + target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(${PROJECT_NAME} PUBLIC $ $) + ament_target_dependencies(${PROJECT_NAME} mesh_map LVR2) target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_LAYERS_BUILDING_LIBRARY") +target_link_libraries(${PROJECT_NAME} + ${LVR2_LIBRARIES} +) install(DIRECTORY include/ DESTINATION include @@ -28,7 +38,6 @@ install(DIRECTORY include/ install(TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -36,6 +45,5 @@ install(TARGETS ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies(mesh_map) ament_package() \ No newline at end of file diff --git a/mesh_layers/src/border_layer.cpp b/mesh_layers/src/border_layer.cpp index b420a415..4943dd66 100644 --- a/mesh_layers/src/border_layer.cpp +++ b/mesh_layers/src/border_layer.cpp @@ -48,7 +48,8 @@ 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 >(layer_name_); + auto mesh_io = map_ptr_->meshIO(); + auto border_costs_opt = mesh_io->getDenseAttributeMap >(layer_name_); if (border_costs_opt) { @@ -79,7 +80,8 @@ 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_, layer_name_)) + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(border_costs_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved border costs to map file."); return true; @@ -98,7 +100,7 @@ float BorderLayer::threshold() bool BorderLayer::computeLayer() { - border_costs_ = lvr2::calcBorderCosts(*mesh_ptr_, config_.border_cost); + border_costs_ = lvr2::calcBorderCosts(*(map_ptr_->mesh()), config_.border_cost); return computeLethals(); } diff --git a/mesh_layers/src/height_diff_layer.cpp b/mesh_layers/src/height_diff_layer.cpp index f0c2c90d..b1866fce 100644 --- a/mesh_layers/src/height_diff_layer.cpp +++ b/mesh_layers/src/height_diff_layer.cpp @@ -48,7 +48,8 @@ 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>(layer_name_); + auto mesh_io = map_ptr_->meshIO(); + auto height_diff_opt = mesh_io->getDenseAttributeMap>(layer_name_); if (height_diff_opt) { @@ -78,7 +79,8 @@ 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_, layer_name_)) + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(height_diff_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved height differences to map file."); return true; @@ -97,7 +99,8 @@ float HeightDiffLayer::threshold() bool HeightDiffLayer::computeLayer() { - height_diff_ = lvr2::calcVertexHeightDifferences(*mesh_ptr_, config_.radius); + auto mesh = map_ptr_->mesh(); + height_diff_ = lvr2::calcVertexHeightDifferences(*mesh, config_.radius); return computeLethals(); } diff --git a/mesh_layers/src/inflation_layer.cpp b/mesh_layers/src/inflation_layer.cpp index 35635e0a..1dbfb0fb 100644 --- a/mesh_layers/src/inflation_layer.cpp +++ b/mesh_layers/src/inflation_layer.cpp @@ -50,8 +50,8 @@ bool InflationLayer::readLayer() { // riskiness RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read riskiness from map file..."); - auto riskiness_opt = mesh_io_ptr_->getDenseAttributeMap>(layer_name_); - + auto mesh_io = map_ptr_->meshIO(); + auto riskiness_opt = mesh_io->getDenseAttributeMap>(layer_name_); if (riskiness_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Riskiness has been read successfully."); @@ -68,7 +68,8 @@ bool InflationLayer::writeLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Saving " << riskiness_.numValues() << " riskiness values to map file..."); - if (mesh_io_ptr_->addDenseAttributeMap(riskiness_, layer_name_)) + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(riskiness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved riskiness to map file."); return true; @@ -161,7 +162,7 @@ inline bool InflationLayer::waveFrontUpdate(lvr2::DenseVertexMap& distanc const lvr2::VertexHandle& v1h, const lvr2::VertexHandle& v2h, const lvr2::VertexHandle& v3h) { - const auto& mesh = map_ptr_->mesh(); + const auto mesh = map_ptr_->mesh(); const double u1 = distances_[v1h]; const double u2 = distances_[v2h]; @@ -170,15 +171,15 @@ inline bool InflationLayer::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u3 == 0) return false; - const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1h, v2h); + const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1h, v2h); const float c = edge_weights[e12h.unwrap()]; const float c_sq = c * c; - const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1h, v3h); + const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1h, v3h); const float b = edge_weights[e13h.unwrap()]; const float b_sq = b * b; - const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2h, v3h); + const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2h, v3h); const float a = edge_weights[e23h.unwrap()]; const float a_sq = a * a; @@ -193,9 +194,9 @@ inline bool InflationLayer::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u1 == 0 && u2 == 0) { - const auto& v1 = mesh_ptr_->getVertexPosition(v1h); - const auto& v2 = mesh_ptr_->getVertexPosition(v2h); - const auto& v3 = mesh_ptr_->getVertexPosition(v3h); + const auto& v1 = mesh->getVertexPosition(v1h); + const auto& v2 = mesh->getVertexPosition(v2h); + const auto& v3 = mesh->getVertexPosition(v3h); const auto& dir = ((v3 - v2) + (v3 - v1)).normalized(); const auto& face_normals = map_ptr_->faceNormals(); cutting_faces_.insert(v1h, fh); @@ -257,30 +258,31 @@ void InflationLayer::waveCostInflation(const std::set& letha const float inscribed_radius, const float inscribed_value, const float lethal_value) { - if (mesh_ptr_) + if (map_ptr_->mesh()) { - auto const& mesh = *mesh_ptr_; + // auto const& mesh = *map_ptr_->mesh(); + const auto mesh = map_ptr_->mesh(); RCLCPP_INFO_STREAM(node_->get_logger(), "inflation radius:" << inflation_radius); RCLCPP_INFO_STREAM(node_->get_logger(), "Init wave inflation."); - lvr2::DenseVertexMap seen(mesh_ptr_->nextVertexIndex(), false); - distances_ = lvr2::DenseVertexMap(mesh_ptr_->nextVertexIndex(), std::numeric_limits::infinity()); + lvr2::DenseVertexMap seen(mesh->nextVertexIndex(), false); + distances_ = lvr2::DenseVertexMap(mesh->nextVertexIndex(), std::numeric_limits::infinity()); lvr2::DenseVertexMap predecessors; - predecessors.reserve(mesh_ptr_->nextVertexIndex()); + predecessors.reserve(mesh->nextVertexIndex()); - vector_map_ = lvr2::DenseVertexMap>(mesh.nextVertexIndex(), lvr2::BaseVector()); + vector_map_ = lvr2::DenseVertexMap>(mesh->nextVertexIndex(), lvr2::BaseVector()); direction_ = lvr2::DenseVertexMap(); const auto& edge_distances = map_ptr_->edgeDistances(); const auto& face_normals = map_ptr_->faceNormals(); - lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); + lvr2::DenseVertexMap fixed(mesh->nextVertexIndex(), false); // initialize distances with infinity // initialize predecessor of each vertex with itself - for (auto const& vH : mesh.vertices()) + for (auto const& vH : mesh->vertices()) { predecessors.insert(vH, vH); } @@ -301,7 +303,7 @@ void InflationLayer::waveCostInflation(const std::set& letha { lvr2::VertexHandle current_vh = pq.popMin().key(); - if (current_vh.idx() >= mesh.nextVertexIndex()) + if (current_vh.idx() >= map_ptr_->mesh()->nextVertexIndex()) { continue; } @@ -316,7 +318,7 @@ void InflationLayer::waveCostInflation(const std::set& letha std::vector neighbours; try { - mesh.getNeighboursOfVertex(current_vh, neighbours); + mesh->getNeighboursOfVertex(current_vh, neighbours); } catch (lvr2::PanicException exception) { @@ -334,7 +336,7 @@ void InflationLayer::waveCostInflation(const std::set& letha std::vector faces; try { - mesh.getFacesOfVertex(nh, faces); + mesh->getFacesOfVertex(nh, faces); } catch (lvr2::PanicException exception) { @@ -344,7 +346,7 @@ void InflationLayer::waveCostInflation(const std::set& letha for (auto fh : faces) { - const auto vertices = mesh.getVerticesOfFace(fh); + const auto vertices = mesh->getVerticesOfFace(fh); const lvr2::VertexHandle& a = vertices[0]; const lvr2::VertexHandle& b = vertices[1]; const lvr2::VertexHandle& c = vertices[2]; @@ -407,7 +409,7 @@ void InflationLayer::waveCostInflation(const std::set& letha RCLCPP_INFO_STREAM(node_->get_logger(), "Finished inflation wave front propagation."); - for (auto vH : mesh_ptr_->vertices()) + for (auto vH : mesh->vertices()) { riskiness_.insert(vH, fading(distances_[vH])); } @@ -495,6 +497,8 @@ void InflationLayer::backToSource(const lvr2::VertexHandle& current_vertex, const lvr2::DenseVertexMap& predecessors, lvr2::DenseVertexMap>& vector_map) { + const auto mesh = map_ptr_->mesh(); + if (vector_map.containsKey(current_vertex)) return; @@ -503,8 +507,8 @@ void InflationLayer::backToSource(const lvr2::VertexHandle& current_vertex, if (pre != current_vertex) { backToSource(pre, predecessors, vector_map); - const auto& v0 = mesh_ptr_->getVertexPosition(current_vertex); - const auto& v1 = mesh_ptr_->getVertexPosition(pre); + const auto& v0 = mesh->getVertexPosition(current_vertex); + const auto& v1 = mesh->getVertexPosition(pre); vector_map.insert(current_vertex, v1 - v0 + vector_map[pre]); } else @@ -532,12 +536,14 @@ void InflationLayer::lethalCostInflation(const std::set& let } }; - lvr2::DenseVertexMap seen(mesh_ptr_->nextVertexIndex(), false); - lvr2::DenseVertexMap distances(mesh_ptr_->nextVertexIndex(), std::numeric_limits::max()); + const auto mesh = map_ptr_->mesh(); + + lvr2::DenseVertexMap seen(mesh->nextVertexIndex(), false); + lvr2::DenseVertexMap distances(mesh->nextVertexIndex(), std::numeric_limits::max()); lvr2::DenseVertexMap prev; - prev.reserve(mesh_ptr_->nextVertexIndex()); + prev.reserve(mesh->nextVertexIndex()); - for (auto vH : mesh_ptr_->vertices()) + for (auto vH : mesh->vertices()) { prev.insert(vH, vH); } @@ -564,12 +570,12 @@ void InflationLayer::lethalCostInflation(const std::set& let seen[vH] = true; std::vector neighbours; - mesh_ptr_->getNeighboursOfVertex(vH, neighbours); + mesh->getNeighboursOfVertex(vH, neighbours); for (auto n : neighbours) { if (distances[n] == 0 || seen[n]) continue; - float n_dist = mesh_ptr_->getVertexPosition(n).squaredDistanceFrom(mesh_ptr_->getVertexPosition(prev[vH])); + float n_dist = mesh->getVertexPosition(n).squaredDistanceFrom(mesh->getVertexPosition(prev[vH])); if (n_dist < distances[n] && n_dist < inflation_radius_squared) { prev[n] = prev[vH]; @@ -579,7 +585,7 @@ void InflationLayer::lethalCostInflation(const std::set& let } } - for (auto vH : mesh_ptr_->vertices()) + for (auto vH : mesh->vertices()) { if (distances[vH] > inflation_radius_squared) { diff --git a/mesh_layers/src/ridge_layer.cpp b/mesh_layers/src/ridge_layer.cpp index 7e930eb9..ee3c4db8 100644 --- a/mesh_layers/src/ridge_layer.cpp +++ b/mesh_layers/src/ridge_layer.cpp @@ -50,7 +50,8 @@ 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>(layer_name_); + auto mesh_io = map_ptr_->meshIO(); + auto ridge_opt = mesh_io->getDenseAttributeMap>(layer_name_); if (ridge_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read ridge from map file."); @@ -63,7 +64,8 @@ bool RidgeLayer::readLayer() bool RidgeLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(ridge_, layer_name_)) + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(ridge_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved ridge to map file."); return true; @@ -99,7 +101,9 @@ bool RidgeLayer::computeLayer() lvr2::DenseFaceMap face_normals; - auto face_normals_opt = mesh_io_ptr_->getDenseAttributeMap>("face_normals"); + const auto mesh = map_ptr_->mesh(); + auto mesh_io = map_ptr_->meshIO(); + auto face_normals_opt = mesh_io->getDenseAttributeMap>("face_normals"); if (face_normals_opt) { @@ -109,9 +113,9 @@ bool RidgeLayer::computeLayer() else { RCLCPP_INFO_STREAM(node_->get_logger(), "No face normals found in the given map file, computing them..."); - face_normals = lvr2::calcFaceNormals(*mesh_ptr_); + face_normals = lvr2::calcFaceNormals(*mesh); RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() << " face normals."); - if (mesh_io_ptr_->addDenseAttributeMap(face_normals, "face_normals")) + if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); } @@ -123,7 +127,7 @@ bool RidgeLayer::computeLayer() } lvr2::DenseVertexMap vertex_normals; - auto vertex_normals_opt = mesh_io_ptr_->getDenseAttributeMap>("vertex_normals"); + auto vertex_normals_opt = mesh_io->getDenseAttributeMap>("vertex_normals"); if (vertex_normals_opt) { @@ -133,8 +137,8 @@ bool RidgeLayer::computeLayer() else { RCLCPP_INFO_STREAM(node_->get_logger(), "No vertex normals found in the given map file, computing them..."); - vertex_normals = lvr2::calcVertexNormals(*mesh_ptr_, face_normals); - if (mesh_io_ptr_->addDenseAttributeMap(vertex_normals, "vertex_normals")) + vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); + if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); } @@ -145,12 +149,12 @@ bool RidgeLayer::computeLayer() } } - ridge_.reserve(mesh_ptr_->nextVertexIndex()); + ridge_.reserve(mesh->nextVertexIndex()); - for (size_t i = 0; i < mesh_ptr_->nextVertexIndex(); i++) + for (size_t i = 0; i < mesh->nextVertexIndex(); i++) { auto vH = lvr2::VertexHandle(i); - if (!mesh_ptr_->containsVertex(vH)) + if (!mesh->containsVertex(vH)) { ridge_.insert(vH, config_.threshold + 0.1); continue; @@ -160,9 +164,9 @@ bool RidgeLayer::computeLayer() float value = 0.0; int num_neighbours = 0; - lvr2::BaseVector reference = mesh_ptr_->getVertexPosition(vH) + vertex_normals[vH]; - visitLocalVertexNeighborhood(*mesh_ptr_.get(), invalid, vH, config_.radius, [&](auto vertex) { - lvr2::BaseVector current_point = mesh_ptr_->getVertexPosition(vertex) + vertex_normals[vertex]; + lvr2::BaseVector reference = mesh->getVertexPosition(vH) + vertex_normals[vH]; + visitLocalVertexNeighborhood(*mesh, invalid, vH, config_.radius, [&](auto vertex) { + lvr2::BaseVector current_point = mesh->getVertexPosition(vertex) + vertex_normals[vertex]; value += (current_point - reference).length(); num_neighbours++; }); diff --git a/mesh_layers/src/roughness_layer.cpp b/mesh_layers/src/roughness_layer.cpp index b6f31b71..479424d8 100644 --- a/mesh_layers/src/roughness_layer.cpp +++ b/mesh_layers/src/roughness_layer.cpp @@ -47,8 +47,9 @@ namespace mesh_layers { bool RoughnessLayer::readLayer() { RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read roughness from map file..."); + auto mesh_io = map_ptr_->meshIO(); auto roughness_opt = - mesh_io_ptr_->getDenseAttributeMap>( + mesh_io->getDenseAttributeMap>( layer_name_); if (roughness_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read roughness from map file."); @@ -60,7 +61,8 @@ bool RoughnessLayer::readLayer() { } bool RoughnessLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(roughness_, layer_name_)) { + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(roughness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved roughness to map file."); return true; } else { @@ -88,8 +90,11 @@ bool RoughnessLayer::computeLayer() { lvr2::DenseFaceMap face_normals; + const auto mesh = map_ptr_->mesh(); + auto mesh_io = map_ptr_->meshIO(); + auto face_normals_opt = - mesh_io_ptr_->getDenseAttributeMap>( + mesh_io->getDenseAttributeMap>( "face_normals"); if (face_normals_opt) { @@ -99,10 +104,10 @@ bool RoughnessLayer::computeLayer() { } else { RCLCPP_INFO_STREAM(node_->get_logger(), "No face normals found in the given map file, computing them..."); - face_normals = lvr2::calcFaceNormals(*mesh_ptr_); + face_normals = lvr2::calcFaceNormals(*mesh); RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() << " face normals."); - if (mesh_io_ptr_->addDenseAttributeMap(face_normals, "face_normals")) { + if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); } else { RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save face normals to map file!"); @@ -112,7 +117,7 @@ bool RoughnessLayer::computeLayer() { lvr2::DenseVertexMap vertex_normals; auto vertex_normals_opt = - mesh_io_ptr_->getDenseAttributeMap>( + mesh_io->getDenseAttributeMap>( "vertex_normals"); if (vertex_normals_opt) { @@ -122,8 +127,8 @@ bool RoughnessLayer::computeLayer() { } else { RCLCPP_INFO_STREAM(node_->get_logger(), "No vertex normals found in the given map file, computing them..."); - vertex_normals = lvr2::calcVertexNormals(*mesh_ptr_, face_normals); - if (mesh_io_ptr_->addDenseAttributeMap(vertex_normals, "vertex_normals")) { + vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); + if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); } else { RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save vertex normals to map file!"); @@ -132,7 +137,7 @@ bool RoughnessLayer::computeLayer() { } roughness_ = - lvr2::calcVertexRoughness(*mesh_ptr_, config_.radius, vertex_normals); + lvr2::calcVertexRoughness(*mesh, config_.radius, vertex_normals); return computeLethals(); } diff --git a/mesh_layers/src/steepness_layer.cpp b/mesh_layers/src/steepness_layer.cpp index 65be9764..1cf38342 100644 --- a/mesh_layers/src/steepness_layer.cpp +++ b/mesh_layers/src/steepness_layer.cpp @@ -49,7 +49,8 @@ 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>(layer_name_); + auto mesh_io = map_ptr_->meshIO(); + auto steepness_opt = mesh_io->getDenseAttributeMap>(layer_name_); if (steepness_opt) { RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read steepness from map file."); @@ -62,7 +63,8 @@ bool SteepnessLayer::readLayer() bool SteepnessLayer::writeLayer() { - if (mesh_io_ptr_->addDenseAttributeMap(steepness_, layer_name_)) + auto mesh_io = map_ptr_->meshIO(); + if (mesh_io->addDenseAttributeMap(steepness_, layer_name_)) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved steepness to map file."); return true; @@ -98,7 +100,9 @@ bool SteepnessLayer::computeLayer() lvr2::DenseFaceMap face_normals; - auto face_normals_opt = mesh_io_ptr_->getDenseAttributeMap>("face_normals"); + const auto mesh = map_ptr_->mesh(); + auto mesh_io = map_ptr_->meshIO(); + auto face_normals_opt = mesh_io->getDenseAttributeMap>("face_normals"); if (face_normals_opt) { @@ -108,9 +112,9 @@ bool SteepnessLayer::computeLayer() else { RCLCPP_INFO_STREAM(node_->get_logger(), "No face normals found in the given map file, computing them..."); - face_normals = lvr2::calcFaceNormals(*mesh_ptr_); + face_normals = lvr2::calcFaceNormals(*mesh); RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() << " face normals."); - if (mesh_io_ptr_->addDenseAttributeMap(face_normals, "face_normals")) + if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); } @@ -122,7 +126,7 @@ bool SteepnessLayer::computeLayer() } lvr2::DenseVertexMap vertex_normals; - auto vertex_normals_opt = mesh_io_ptr_->getDenseAttributeMap>("vertex_normals"); + auto vertex_normals_opt = mesh_io->getDenseAttributeMap>("vertex_normals"); if (vertex_normals_opt) { @@ -132,8 +136,8 @@ bool SteepnessLayer::computeLayer() else { RCLCPP_INFO_STREAM(node_->get_logger(), "No vertex normals found in the given map file, computing them..."); - vertex_normals = lvr2::calcVertexNormals(*mesh_ptr_, face_normals); - if (mesh_io_ptr_->addDenseAttributeMap(vertex_normals, "vertex_normals")) + vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); + if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) { RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); } @@ -144,12 +148,12 @@ bool SteepnessLayer::computeLayer() } } - steepness_.reserve(mesh_ptr_->nextVertexIndex()); + steepness_.reserve(mesh->nextVertexIndex()); - for (size_t i = 0; i < mesh_ptr_->nextVertexIndex(); i++) + for (size_t i = 0; i < mesh->nextVertexIndex(); i++) { auto vH = lvr2::VertexHandle(i); - if (!mesh_ptr_->containsVertex(vH)) + if (!mesh->containsVertex(vH)) { continue; } diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index 86514813..b7ebd292 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(ament_cmake_ros REQUIRED) # ROS Deps set(dependencies geometry_msgs - mesh_client mesh_msgs_conversions pluginlib rclcpp @@ -27,6 +26,7 @@ pkg_check_modules(JSONCPP jsoncpp) include_directories( include + ${LVR2_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -36,13 +36,11 @@ add_library(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC $ $ - ${LVR2_INCLUDE_DIRS} ${ASSIMP_INCLUDE_DIRS}) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) target_link_libraries(${PROJECT_NAME} ${LVR2_LIBRARIES} ${ASSIMP_LIBRARIES} - ${MPI_CXX_LIBRARIES} ) install(DIRECTORY include/ diff --git a/mesh_map/include/mesh_map/abstract_layer.h b/mesh_map/include/mesh_map/abstract_layer.h index 8ae0f39d..892abc9c 100644 --- a/mesh_map/include/mesh_map/abstract_layer.h +++ b/mesh_map/include/mesh_map/abstract_layer.h @@ -159,18 +159,14 @@ class AbstractLayer 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) + std::shared_ptr map, + const rclcpp::Node::SharedPtr node) { layer_name_ = name; node_ = node; layer_namespace_ = "mesh_map/" + name; notify_ = notify_update; - mesh_ptr_ = mesh; map_ptr_ = map; - mesh_io_ptr_ = io; return initialize(); } @@ -181,8 +177,6 @@ class AbstractLayer protected: std::string layer_name_; - std::shared_ptr mesh_io_ptr_; - std::shared_ptr> mesh_ptr_; std::shared_ptr map_ptr_; rclcpp::Node::SharedPtr node_; diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 5f0a9880..77be5cc8 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -43,8 +43,8 @@ #include #include #include -#include -#include +#include + #include #include #include @@ -55,12 +55,17 @@ #include #include +#include +#include +#include + #include "nanoflann.hpp" #include "nanoflann_mesh_adaptor.h" + namespace mesh_map { -class MeshMap +class MeshMap : public std::enable_shared_from_this { public: inline static const std::string MESH_MAP_NAMESPACE = "mesh_map"; @@ -270,9 +275,17 @@ class MeshMap /** * @brief Returns the stored mesh */ - const lvr2::HalfEdgeMesh& mesh() + std::shared_ptr > mesh() { - return *mesh_ptr; + return mesh_ptr; + } + + /** + * @brief Returns the mesh-io object + */ + std::shared_ptr meshIO() + { + return mesh_io_ptr; } /** @@ -420,7 +433,7 @@ class MeshMap //! 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; + std::shared_ptr> mesh_ptr; lvr2::DenseVertexMap invalid; private: @@ -528,9 +541,6 @@ class MeshMap nanoflann::L2_Simple_Adaptor, NanoFlannMeshAdaptor, 3> KDTree; - //! k-d tree nano flann mesh adaptor to access mesh data - std::unique_ptr adaptor_ptr; - //! k-d tree to query mesh vertices in logarithmic time std::unique_ptr kd_tree_ptr; }; diff --git a/mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h b/mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h index 8421fdf1..fd24b09d 100644 --- a/mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h +++ b/mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h @@ -38,25 +38,25 @@ #ifndef MESH_MAP__NANOFLANN_MESH_ADAPTOR_H #define MESH_MAP__NANOFLANN_MESH_ADAPTOR_H -#include +#include #include "nanoflann.hpp" namespace mesh_map{ struct NanoFlannMeshAdaptor { - const lvr2::HalfEdgeMesh>& mesh; + const std::shared_ptr>> mesh; /// The constructor that sets the data set source - NanoFlannMeshAdaptor(const lvr2::HalfEdgeMesh> &mesh) : mesh(mesh) { } + NanoFlannMeshAdaptor(const std::shared_ptr>> mesh) : mesh(mesh) { } - inline lvr2::Index kdtree_get_point_count() const { return mesh.nextVertexIndex(); } + inline lvr2::Index kdtree_get_point_count() const { return mesh->nextVertexIndex(); } inline float kdtree_get_pt(const lvr2::Index idx, const size_t dim) const { const lvr2::VertexHandle vH(idx); - if(mesh.containsVertex(vH)) + if(mesh->containsVertex(vH)) { - const lvr2::BaseVector& vertex = mesh.getVertexPosition(vH); + const lvr2::BaseVector& vertex = mesh->getVertexPosition(vH); if (dim == 0) return vertex.x; else if (dim == 1) return vertex.y; else return vertex.z; diff --git a/mesh_map/include/mesh_map/util.h b/mesh_map/include/mesh_map/util.h index 71a7c59c..21e9a2a9 100644 --- a/mesh_map/include/mesh_map/util.h +++ b/mesh_map/include/mesh_map/util.h @@ -48,7 +48,7 @@ #include #include -#include +#include #include namespace mesh_map diff --git a/mesh_map/package.xml b/mesh_map/package.xml index f1c4864e..9772bb32 100644 --- a/mesh_map/package.xml +++ b/mesh_map/package.xml @@ -9,7 +9,6 @@ Sebastian Pütz geometry_msgs - mesh_client mesh_msgs_conversions pluginlib rclcpp diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 47cd3e71..9fd5abac 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -39,16 +39,19 @@ #include #include #include +#include + #include #include #include #include -#include #include #include #include -#include +#include +#include + #include #include #include @@ -245,16 +248,15 @@ bool MeshMap::readMap() 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) + if(auto mesh_opt = mesh_io_ptr->getMesh()) { *mesh_ptr = mesh_opt.get(); 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)); + // adaptor_ptr = std::make_unique(mesh_ptr); + kd_tree_ptr = std::make_unique(3, NanoFlannMeshAdaptor(mesh_ptr), nanoflann::KDTreeSingleIndexAdaptorParams(10)); kd_tree_ptr->buildIndex(); RCLCPP_INFO_STREAM(node->get_logger(), "The k-d tree has been build successfully!"); } @@ -326,7 +328,7 @@ bool MeshMap::readMap() sleep(0.5); map_stamp = node->now(); } - mesh_geometry_pub->publish(mesh_msgs_conversions::toMeshGeometryStamped(*mesh_ptr, global_frame, uuid_str, vertex_normals, map_stamp)); + mesh_geometry_pub->publish(mesh_msgs_conversions::toMeshGeometryStamped(mesh_ptr, global_frame, uuid_str, vertex_normals, map_stamp)); publishVertexColors(map_stamp); RCLCPP_INFO_STREAM(node->get_logger(), "Try to read edge distances from map file..."); @@ -450,8 +452,6 @@ bool MeshMap::initLayerPlugins() lethals.clear(); lethal_indices.clear(); - std::shared_ptr map(this); - for (auto& layer : loaded_layers) { auto& layer_plugin = layer.second; @@ -459,7 +459,7 @@ bool MeshMap::initLayerPlugins() auto callback = [this](const std::string& layer_name) { layerChanged(layer_name); }; - if (!layer_plugin->initialize(layer_name, callback, map, mesh_ptr, mesh_io_ptr, node)) + if (!layer_plugin->initialize(layer_name, callback, shared_from_this(), node)) { RCLCPP_ERROR_STREAM(node->get_logger(), "Could not initialize the layer plugin with the name \"" << layer_name << "\"!"); return false; @@ -825,7 +825,7 @@ void MeshMap::publishVectorField(const std::string& name, const lvr2::DenseVertexMap& values, const std::function& cost_function, const bool publish_face_vectors) { - const auto& mesh = this->mesh(); + const auto mesh = this->mesh(); const auto& vertex_costs = vertexCosts(); const auto& face_normals = faceNormals(); @@ -851,7 +851,7 @@ void MeshMap::publishVectorField(const std::string& name, unsigned int cnt = 0; unsigned int faces = 0; - lvr2::DenseFaceMap vector_field_faces(mesh.numFaces(), 0); + lvr2::DenseFaceMap vector_field_faces(mesh->numFaces(), 0); std::set complete_faces; for (auto vH : vector_map) @@ -864,7 +864,7 @@ void MeshMap::publishVectorField(const std::string& name, continue; } - auto u = mesh.getVertexPosition(vH); + auto u = mesh->getVertexPosition(vH); auto v = u + dir_vec * 0.1; u.z = u.z + 0.01; @@ -889,7 +889,7 @@ void MeshMap::publishVectorField(const std::string& name, // vector_field.markers.push_back(vector); try { - for (auto fH : mesh.getFacesOfVertex(vH)) + for (auto fH : mesh->getFacesOfVertex(vH)) { if (++vector_field_faces[fH] == 3) { @@ -923,8 +923,8 @@ void MeshMap::publishVectorField(const std::string& name, for (auto fH : complete_faces) { - const auto& vertices = mesh.getVertexPositionsOfFace(fH); - const auto& vertex_handles = mesh.getVerticesOfFace(fH); + const auto& vertices = mesh->getVertexPositionsOfFace(fH); + const auto& vertex_handles = mesh->getVerticesOfFace(fH); mesh_map::Vector center = (vertices[0] + vertices[1] + vertices[2]) / 3; std::array barycentric_coords; float dist; From 37abc56d44073e7daf68f865605d334990eeccfc Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 6 Dec 2024 16:54:26 +0100 Subject: [PATCH 3/9] almost the same state as before --- mesh_map/src/mesh_map.cpp | 29 +++++++++++++++++++++-------- mesh_map/src/util.cpp | 7 ++++++- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 9fd5abac..a4f13a4a 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -163,7 +163,12 @@ bool MeshMap::readMap() { if(!mesh_io_ptr) { - if(!mesh_file.empty() && !mesh_part.empty()) + if(mesh_file.empty()) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open file connection!"); + return false; + } + else { if(mesh_working_file == "") { @@ -225,32 +230,40 @@ bool MeshMap::readMap() RCLCPP_ERROR_STREAM(node->get_logger(), "Error while loading map: " << io.GetErrorString()); return false; } + RCLCPP_INFO_STREAM(node->get_logger(), "extractMeshByName"); mesh_buffer = extractMeshByName(ascene, mesh_part); } if(!mesh_buffer) { RCLCPP_ERROR_STREAM(node->get_logger(), "Couldn't load mesh part: '" << mesh_part << "'"); + return false; } + RCLCPP_INFO_STREAM(node->get_logger(), "Loaded mesh buffer: \n" << *mesh_buffer); + // 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 << "'..."); - if(auto mesh_opt = mesh_io_ptr->getMesh()) + auto hdf5_mesh_input = std::make_shared(); + hdf5_mesh_input->open(mesh_working_file); + hdf5_mesh_input->setMeshName(mesh_working_part); + lvr2::MeshBufferPtr mesh_buffer = hdf5_mesh_input->MeshIO::load(mesh_working_part); + + RCLCPP_INFO_STREAM(node->get_logger(), "Convert buffer to HEM: \n" << *mesh_buffer); + + // auto mesh_opt = mesh_io_ptr->getMesh(); + if(mesh_buffer) { - *mesh_ptr = mesh_opt.get(); + mesh_ptr = std::make_shared >(mesh_buffer); 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."); diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index c5b2587f..e04bd6c6 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -49,7 +49,6 @@ namespace fs = std::filesystem; namespace mesh_map { -// again this function std::vector split(std::string s, const std::string& delimiter) { std::vector tokens; @@ -100,6 +99,9 @@ lvr2::MeshBufferPtr extractMeshByName( const aiScene* ascene, std::string name) { + std::cout << "extractMeshByName" << std::endl; + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "HELLOO"); + lvr2::MeshBufferPtr mesh; const aiNode* root_node = ascene->mRootNode; @@ -112,12 +114,15 @@ lvr2::MeshBufferPtr extractMeshByName( if(path_to_mesh.size() == 0) { + std::cout << "path empty!" << std::endl; return mesh; } const aiNode* node_it = root_node; for(size_t i=1; i Date: Mon, 9 Dec 2024 16:47:56 +0100 Subject: [PATCH 4/9] You can select the half-edge-mesh implementation via the parameter 'hem' now. 'pmp' and 'lvr' are implemented. The old half-edge-mesh can be selected by choosing 'lvr'. The new default half-edge-mesh is 'pmp' since it is more robust to different source meshes. The 'pmp' hem worked also to load all available meshes dynamically. Cleanup: I removed mesh_client package since it was used nowhere. --- cvp_mesh_planner/CMakeLists.txt | 7 + cvp_mesh_planner/src/cvp_mesh_planner.cpp | 22 +- dijkstra_mesh_planner/CMakeLists.txt | 7 + mbf_mesh_core/CMakeLists.txt | 6 + mbf_mesh_nav/CMakeLists.txt | 7 + mesh_client/CHANGELOG.rst | 15 - mesh_client/CMakeLists.txt | 50 --- mesh_client/COLCON_IGNORE | 0 mesh_client/include/mesh_client/mesh_client.h | 187 ---------- mesh_client/package.xml | 23 -- mesh_client/src/mesh_client.cpp | 350 ------------------ mesh_controller/CMakeLists.txt | 8 +- mesh_controller/src/mesh_controller.cpp | 3 +- mesh_layers/CMakeLists.txt | 7 + mesh_map/CMakeLists.txt | 7 + mesh_map/include/mesh_map/mesh_map.h | 8 +- mesh_map/src/mesh_map.cpp | 81 +++- mesh_map/src/util.cpp | 7 +- 18 files changed, 139 insertions(+), 656 deletions(-) delete mode 100644 mesh_client/CHANGELOG.rst delete mode 100644 mesh_client/CMakeLists.txt delete mode 100644 mesh_client/COLCON_IGNORE delete mode 100644 mesh_client/include/mesh_client/mesh_client.h delete mode 100644 mesh_client/package.xml delete mode 100644 mesh_client/src/mesh_client.cpp diff --git a/cvp_mesh_planner/CMakeLists.txt b/cvp_mesh_planner/CMakeLists.txt index 05712ea0..876769ea 100644 --- a/cvp_mesh_planner/CMakeLists.txt +++ b/cvp_mesh_planner/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.8) project(cvp_mesh_planner) +# 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_ros REQUIRED) find_package(mbf_mesh_core REQUIRED) find_package(mbf_msgs REQUIRED) diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 8e259ade..44b61969 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -70,13 +70,15 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, // mesh_map->combineVertexCosts(); // TODO should be outside the planner - RCLCPP_INFO(node_->get_logger(), "start wave front propagation."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "start wave front propagation."); mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position); mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position); const uint32_t outcome = waveFrontPropagation(goal_vec, start_vec, path, message); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "finished wave front propagation."); + path.reverse(); std_msgs::msg::Header header; @@ -649,15 +651,24 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto& vertex_costs = mesh_map_->vertexCosts(); auto& invalid = mesh_map_->invalid; + + RCLCPP_DEBUG_STREAM(node_->get_logger(), "BLA."); + mesh_map_->publishDebugPoint(original_start, mesh_map::color(0, 1, 0), "start_point"); mesh_map_->publishDebugPoint(original_goal, mesh_map::color(0, 0, 1), "goal_point"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "published point."); + mesh_map::Vector start = original_start; mesh_map::Vector goal = original_goal; + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Find face."); + // Find the containing faces of start and goal - const auto& start_opt = mesh_map_->getContainingFace(start, 0.4); - const auto& goal_opt = mesh_map_->getContainingFace(goal, 0.4); + const lvr2::OptionalFaceHandle start_opt = mesh_map_->getContainingFace(start, 0.4); + const lvr2::OptionalFaceHandle goal_opt = mesh_map_->getContainingFace(goal, 0.4); + + RCLCPP_DEBUG_STREAM(node_->get_logger(), "BLA 2."); const auto t_initialization_start = std::chrono::steady_clock::now(); @@ -666,10 +677,12 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s if (!start_opt) { message = "Could not find a face close enough to the given start pose"; + RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message); return mbf_msgs::action::GetPath::Result::INVALID_START; } if (!goal_opt) { message = "Could not find a face close enough to the given goal pose"; + RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message); return mbf_msgs::action::GetPath::Result::INVALID_GOAL; } @@ -688,6 +701,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s // clear vector field map vector_map_.clear(); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init distances."); // initialize distances with infinity // initialize predecessor of each vertex with itself for (auto const& vH : mesh->vertices()) @@ -727,6 +741,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto t_wavefront_start = std::chrono::steady_clock::now(); const auto initialization_duration_ms = std::chrono::duration_cast(t_wavefront_start - t_initialization_start); + + RCLCPP_DEBUG_STREAM(node_->get_logger(), "GO."); while (!pq.isEmpty() && !cancel_planning_) { lvr2::VertexHandle current_vh = pq.popMin().key(); diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index f07cbd54..cfd23994 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.8) project(dijkstra_mesh_planner) +# 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_ros REQUIRED) find_package(mbf_mesh_core REQUIRED) find_package(mbf_msgs REQUIRED) diff --git a/mbf_mesh_core/CMakeLists.txt b/mbf_mesh_core/CMakeLists.txt index b9e8233b..8b8c0b45 100644 --- a/mbf_mesh_core/CMakeLists.txt +++ b/mbf_mesh_core/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 3.5) project(mbf_mesh_core) +# 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_ros REQUIRED) set(dependencies diff --git a/mbf_mesh_nav/CMakeLists.txt b/mbf_mesh_nav/CMakeLists.txt index 65558274..1e499dfe 100644 --- a/mbf_mesh_nav/CMakeLists.txt +++ b/mbf_mesh_nav/CMakeLists.txt @@ -5,6 +5,13 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) 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_ros REQUIRED) # ROS deps set(dependencies diff --git a/mesh_client/CHANGELOG.rst b/mesh_client/CHANGELOG.rst deleted file mode 100644 index 545b2ed0..00000000 --- a/mesh_client/CHANGELOG.rst +++ /dev/null @@ -1,15 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mesh_client -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (2024-05-15) ------------------- -* port package to ROS2 - - -1.0.1 (2021-10-27) ------------------- - -1.0.0 (2020-12-16) ------------------- -* Initial release diff --git a/mesh_client/CMakeLists.txt b/mesh_client/CMakeLists.txt deleted file mode 100644 index 92573d85..00000000 --- a/mesh_client/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(mesh_client) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -find_package(ament_cmake_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(LVR2 REQUIRED) -find_package(MPI) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(CURL REQUIRED) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(JSONCPP jsoncpp) - -include_directories( - include -) - -add_library(${PROJECT_NAME} - src/mesh_client.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" - ${LVR2_INCLUDE_DIRS}) -ament_target_dependencies(${PROJECT_NAME} - rclcpp) -target_link_libraries(${PROJECT_NAME} - jsoncpp - Eigen3::Eigen - ${LVR2_LIBRARIES} - ${MPI_CXX_LIBRARIES} -) - -install(DIRECTORY include/ - DESTINATION include -) -install(TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - -ament_export_libraries(${PROJECT_NAME}) -ament_export_include_directories(include) -ament_export_dependencies(${dependencies}) -ament_package() \ No newline at end of file diff --git a/mesh_client/COLCON_IGNORE b/mesh_client/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mesh_client/include/mesh_client/mesh_client.h b/mesh_client/include/mesh_client/mesh_client.h deleted file mode 100644 index 157909aa..00000000 --- a/mesh_client/include/mesh_client/mesh_client.h +++ /dev/null @@ -1,187 +0,0 @@ -/* - * Copyright 2020, Sebastian Pütz - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * authors: - * Sebastian Pütz - * - */ - -#ifndef MESH_CLIENT_H_ -#define MESH_CLIENT_H_ - -// #define CPPHTTPLIB_OPENSSL_SUPPORT - -#include -#include -#include -#include -#include -#include -#include - -namespace mesh_client -{ -class MeshClient : public lvr2::AttributeMeshIOBase -{ -public: - /** - * @brief Constructs a mesh client which receives - * @param server_url The url of the server, use http://localhost/my/path/to/mesh if the server is used locally. - * @param logger The ROS logger that shall be used by the MeshClient. Defaults to using a logger "mesh_client" - */ - MeshClient(const std::string& server_url, const std::string& server_username, const std::string& server_password, - const std::string& mesh_layer, rclcpp::Logger logger = rclcpp::get_logger("mesh_client")); - - /** - * @brief sets the Bounding box for the query which is send to the server - */ - void setBoundingBox(float min_x, float min_y, float min_z, const float max_x, const float max_y, const float max_z); - void addFilter(std::string channel, float min_value, float max_value); - - /** - * @brief Builds a JSON string containing the set bounding - * box and the attribute name and the attribute group - * @param attribute_name The name of the corresponding attribute - * to be requested from the server - * @return the compsoed JSON string following the server's specification - */ - std::string buildJson(const std::string& attribute_name); - - /** - * @brief Defines the data type of the transferred channel to decode the byte buffer. - */ - enum Type : char - { - UINT = 0, - FLOAT = 1, - UCHAR = 2 - }; - - /** - * @brief Persistence layer interface, Accesses the vertices of the mesh in the persistence layer. - * @return An optional float channel, the channel is valid if the mesh vertices have been read successfully - */ - lvr2::FloatChannelOptional getVertices(); - - /** - * @brief Persistence layer interface, Accesses the face indices of the mesh in the persistence layer. - * @return An optional index channel, the channel is valid if the mesh indices have been read successfully - */ - lvr2::IndexChannelOptional getIndices(); - - /** - * @brief Persistence layer interface, Writes the vertices of the mesh to the persistence layer. - * @return true if the channel has been written successfully - */ - bool addVertices(const lvr2::FloatChannel& channel_ptr); - - /** - * @brief Persistence layer interface, Writes the face indices of the mesh to the persistence layer. - * @return true if the channel has been written successfully - */ - bool addIndices(const lvr2::IndexChannel& channel_ptr); - - /** - * @brief getChannel Reads a float attribute channel in the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the float channel - * @return true if the channel has been loaded successfully, false otherwise - */ - bool getChannel(const std::string group, const std::string name, lvr2::FloatChannelOptional& channel); - - /** - * @brief getChannel Reads an index attribute channel in the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the index channel - * @return true if the channel has been loaded successfully, false otherwise - */ - bool getChannel(const std::string group, const std::string name, lvr2::IndexChannelOptional& channel); - - /** - * @brief getChannel Reads an unsigned char attribute channel in the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the unsigned char channel - * @return true if the channel has been loaded successfully, false otherwise - */ - bool getChannel(const std::string group, const std::string name, lvr2::UCharChannelOptional& channel); - - /** - * @brief addChannel Writes a float attribute channel from the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the float channel which should be written - * @return true if the channel has been written successfully, false otherwise - */ - bool addChannel(const std::string group, const std::string name, const lvr2::FloatChannel& channel); - - /** - * @brief addChannel Writes an index attribute channel from the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the index channel which should be written - * @return true if the channel has been written successfully, false otherwise - */ - bool addChannel(const std::string group, const std::string name, const lvr2::IndexChannel& channel); - - /** - * @brief addChannel Writes an unsigned char attribute channel from the given group with the given name - * @param group The associated attribute group - * @param name The associated attribute name - * @param channel The pointer to the unsigned char channel which should be written - * @return true if the channel has been written successfully, false otherwise - */ - bool addChannel(const std::string group, const std::string name, const lvr2::UCharChannel& channel); - -private: - std::unique_ptr requestChannel(std::string channel); - - std::map uchar_channels; - std::map index_channels; - std::map float_channels; - - std::string server_url_; - std::string server_username_; - std::string server_password_; - - std::string mesh_layer_; - std::array bounding_box_; - std::map> mesh_filters_; - - rclcpp::Logger logger_; -}; - -} // namespace mesh_client - -#endif // MESH_CLIENT_H_ diff --git a/mesh_client/package.xml b/mesh_client/package.xml deleted file mode 100644 index 77cad9ee..00000000 --- a/mesh_client/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - mesh_client - 2.0.0 - The mesh_client package - - Matthias Holoch - Sebastian Pütz - BSD-3 - Sebastian Pütz - Malte kl. Piening - - pkg-config - curl - eigen - libjsoncpp-dev - lvr2 - rclcpp - - - ament_cmake - - diff --git a/mesh_client/src/mesh_client.cpp b/mesh_client/src/mesh_client.cpp deleted file mode 100644 index abee4cf7..00000000 --- a/mesh_client/src/mesh_client.cpp +++ /dev/null @@ -1,350 +0,0 @@ -/* - * Copyright 2020, Sebastian Pütz - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * authors: - * Sebastian Pütz - * - */ -#include "mesh_client/mesh_client.h" - -#include -#include - -namespace mesh_client -{ -MeshClient::MeshClient(const std::string& server_url, const std::string& server_username, const std::string& server_password, const std::string& mesh_layer, rclcpp::Logger logger) - : server_url_(server_url) - , server_username_(server_username) - , server_password_(server_password) - , mesh_layer_(mesh_layer) - , logger_(logger) -{ -} - -void MeshClient::setBoundingBox(float min_x, float min_y, float min_z, const float max_x, const float max_y, - const float max_z) -{ - bounding_box_[0] = min_x; - bounding_box_[1] = min_y; - bounding_box_[2] = min_z; - bounding_box_[3] = max_x; - bounding_box_[4] = max_y; - bounding_box_[5] = max_z; -} - -void MeshClient::addFilter(std::string channel, float min_value, float max_value) -{ - mesh_filters_[channel] = std::make_pair(min_value, max_value); -} - -std::string MeshClient::buildJson(const std::string& attribute_name) -{ - Json::Value attr; - attr["name"] = attribute_name; - - if (mesh_filters_.size() > 0) - { - Json::Value filters; - for (auto& mesh_filter : mesh_filters_) - { - Json::Value filter; - - filter["attribute_name"] = mesh_filter.first; - filter["min_val"] = mesh_filter.second.first; - filter["max_val"] = mesh_filter.second.second; - - filters.append(filter); - } - attr["filters"] = filters; - } - - Json::Value json_bb; - json_bb["x_min"] = bounding_box_[0]; - json_bb["y_min"] = bounding_box_[1]; - json_bb["z_min"] = bounding_box_[2]; - json_bb["x_max"] = bounding_box_[3]; - json_bb["y_max"] = bounding_box_[4]; - json_bb["z_max"] = bounding_box_[5]; - - Json::Value request; - request["boundingbox"] = json_bb; - request["attribute"] = attr; - request["layer"] = mesh_layer_; - Json::FastWriter fast_writer; - return fast_writer.write(request); -} - -bool parseByteDataString(std::string string, char& type, unsigned long& size, unsigned long& width, char*& data) -{ - if (string.length() < 10) - { - return false; - } - - // parse body - const char* body = string.c_str(); - type = body[0]; // one byte for type of message - size = *reinterpret_cast(body + 1); // eight bytes for length of message - width = *reinterpret_cast(body + 9); // eight bytes for amount of values per element - data = reinterpret_cast(const_cast(body + 17)); // raw data - - return true; -} - -lvr2::FloatChannelOptional MeshClient::getVertices() -{ - if (float_channels.find("vertices") != float_channels.end()) - { - return float_channels["vertices"]; - } - - unique_ptr str = requestChannel("vertices"); - - if (str && str->size() > 17) - { - char type; - unsigned long size, width; - char* data; - - RCLCPP_DEBUG_STREAM(logger_, "Received vertices channel"); - - if (parseByteDataString(*str, type, size, width, data) && type == Type::FLOAT) - { - float* float_data = reinterpret_cast(data); - auto channel = lvr2::FloatChannel(size, width); - memcpy(channel.dataPtr().get(), float_data, size * width * sizeof(float)); - - return channel; - } - } - RCLCPP_ERROR_STREAM(logger_, "Failed to load vertices channel!"); - return lvr2::FloatChannelOptional(); -} - -lvr2::IndexChannelOptional MeshClient::getIndices() -{ - if (index_channels.find("face_indices") != index_channels.end()) - { - return index_channels["face_indices"]; - } - - unique_ptr str = requestChannel("face_indices"); - - if (str && str->size() > 17) - { - char type; - unsigned long size, width; - char* data; - - RCLCPP_DEBUG_STREAM(logger_, "Received indices channel"); - - if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT) - { - unsigned int* index_data = reinterpret_cast(data); - auto channel = lvr2::IndexChannel(size, width); - memcpy(channel.dataPtr().get(), index_data, size * width * sizeof(lvr2::Index)); - - return channel; - } - } - RCLCPP_ERROR_STREAM(logger_, "Failed to load indices channel!"); - return lvr2::IndexChannelOptional(); -} - -bool MeshClient::addVertices(const lvr2::FloatChannel& channel) -{ - float_channels["vertices"] = channel; - return true; -} - -bool MeshClient::addIndices(const lvr2::IndexChannel& channel) -{ - index_channels["face_indices"] = channel; - return true; -} - -bool MeshClient::getChannel(const std::string group, const std::string name, lvr2::FloatChannelOptional& channel) -{ - std::cout << "channel " << name << std::endl; - if (float_channels.find(name) != float_channels.end()) - { - channel = float_channels[name]; - return true; - } - - unique_ptr str = requestChannel(name); - - if (str && str->size() > 17) - { - char type; - unsigned long size, width; - char* data; - - RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel"); - - if (parseByteDataString(*str, type, size, width, data) && type == Type::FLOAT) - { - float* float_data = reinterpret_cast(data); - channel = lvr2::FloatChannel(size, width); - memcpy(channel.get().dataPtr().get(), float_data, size * width * sizeof(float)); - return true; - } - } - RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!"); - return false; -} - -bool MeshClient::getChannel(const std::string group, const std::string name, lvr2::IndexChannelOptional& channel) -{ - if (index_channels.find(name) != index_channels.end()) - { - channel = index_channels[name]; - return true; - } - - unique_ptr str = requestChannel(name); - - if (str && str->size() > 17) - { - char type; - unsigned long size, width; - char* data; - - RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel"); - - if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT) - { - unsigned int* index_data = reinterpret_cast(data); - channel = lvr2::IndexChannel(size, width); - memcpy(channel.get().dataPtr().get(), index_data, size * width * sizeof(lvr2::Index)); - return true; - } - } - RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!"); - return false; -} - -bool MeshClient::getChannel(const std::string group, const std::string name, lvr2::UCharChannelOptional& channel) -{ - if (uchar_channels.find(name) != uchar_channels.end()) - { - channel = uchar_channels[name]; - return true; - } - - unique_ptr str = requestChannel(name); - - if (str && str->size() > 17) - { - char type; - unsigned long size, width; - char* data; - - RCLCPP_DEBUG_STREAM(logger_, "Received " << name << " channel"); - - if (parseByteDataString(*str, type, size, width, data) && type == Type::UINT) - { - unsigned char* uchar_data = reinterpret_cast(data); - channel = lvr2::UCharChannel(size, width); - memcpy(channel.get().dataPtr().get(), uchar_data, size * width * sizeof(unsigned char)); - return true; - } - } - RCLCPP_ERROR_STREAM(logger_, "Failed to load " << name << " channel!"); - return false; -} - -bool MeshClient::addChannel(const std::string group, const std::string name, const lvr2::FloatChannel& channel) -{ - float_channels[name] = channel; - return true; -} - -bool MeshClient::addChannel(const std::string group, const std::string name, const lvr2::IndexChannel& channel) -{ - index_channels[name] = channel; - return true; -} - -bool MeshClient::addChannel(const std::string group, const std::string name, const lvr2::UCharChannel& channel) -{ - uchar_channels[name] = channel; - return true; -} - -size_t writeFunction(void* ptr, size_t size, size_t nmemb, std::string* data) -{ - data->append((char*)ptr, size * nmemb); - return size * nmemb; -} - -std::unique_ptr MeshClient::requestChannel(std::string channel) -{ - CURL* curl; - - curl_global_init(CURL_GLOBAL_ALL); - - curl = curl_easy_init(); - if (!curl) - { - curl_global_cleanup(); - return nullptr; - } - - std::string post_body = buildJson(channel); - curl_easy_setopt(curl, CURLOPT_URL, server_url_.c_str()); - - struct curl_slist* list = curl_slist_append(list, "Content-Type: application/json"); - - curl_easy_setopt(curl, CURLOPT_HTTPHEADER, list); - curl_easy_setopt(curl, CURLOPT_POSTFIELDS, post_body.c_str()); - curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_ANY); - std::string usr_pwd = server_username_ + ":" + server_password_; - curl_easy_setopt(curl, CURLOPT_USERPWD, usr_pwd.c_str()); - - unique_ptr result = std::make_unique(); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writeFunction); - curl_easy_setopt(curl, CURLOPT_WRITEDATA, result.get()); - - CURLcode res = curl_easy_perform(curl); - if (res != CURLE_OK) - { - std::cout << "error" << std::endl; - curl_easy_cleanup(curl); - return nullptr; - } - - curl_easy_cleanup(curl); - return result; -} - -} /* namespace mesh_client */ diff --git a/mesh_controller/CMakeLists.txt b/mesh_controller/CMakeLists.txt index f7dde9ce..529bcaab 100644 --- a/mesh_controller/CMakeLists.txt +++ b/mesh_controller/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.8) project(mesh_controller) +# 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_ros REQUIRED) find_package(example_interfaces REQUIRED) find_package(mbf_mesh_core REQUIRED) @@ -12,7 +19,6 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(LVR2 REQUIRED) find_package(MPI) - pluginlib_export_plugin_description_file(mbf_mesh_core mesh_controller.xml) add_library(${PROJECT_NAME} src/mesh_controller.cpp) diff --git a/mesh_controller/src/mesh_controller.cpp b/mesh_controller/src/mesh_controller.cpp index b2c86c88..ee7f47c8 100644 --- a/mesh_controller/src/mesh_controller.cpp +++ b/mesh_controller/src/mesh_controller.cpp @@ -144,10 +144,9 @@ uint32_t MeshController::computeVelocityCommands(const geometry_msgs::msg::PoseS } const lvr2::FaceHandle& face = current_face_.unwrap(); - std::array handles = map_ptr_->mesh_ptr->getVerticesOfFace(face); + std::array handles = mesh->getVerticesOfFace(face); // update to which position of the plan the robot is closest - const auto& opt_dir = map_ptr_->directionAtPosition(vector_map_, handles, bary_coords); if (!opt_dir) { diff --git a/mesh_layers/CMakeLists.txt b/mesh_layers/CMakeLists.txt index 6c03ad63..a9a01812 100644 --- a/mesh_layers/CMakeLists.txt +++ b/mesh_layers/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.8) project(mesh_layers) +# 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_ros REQUIRED) find_package(mesh_map REQUIRED) find_package(LVR2 REQUIRED) diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index b7ebd292..f6dec904 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.5) project(mesh_map) +# 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_ros REQUIRED) # ROS Deps set(dependencies diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 77be5cc8..8f7fec81 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -428,14 +428,17 @@ class MeshMap : public std::enable_shared_from_this */ std_srvs::srv::Trigger::Response writeLayers(); + lvr2::DenseVertexMap invalid; + +protected: //! This is an abstract interface to load mesh information from somewhere //! 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; std::shared_ptr> mesh_ptr; + std::string hem_impl_; - lvr2::DenseVertexMap invalid; private: //! plugin class loader for for the layer plugins pluginlib::ClassLoader layer_loader; @@ -541,6 +544,9 @@ class MeshMap : public std::enable_shared_from_this nanoflann::L2_Simple_Adaptor, NanoFlannMeshAdaptor, 3> KDTree; + //! k-d tree nano flann mesh adaptor to access mesh data + std::unique_ptr adaptor_ptr; + //! k-d tree to query mesh vertices in logarithmic time std::unique_ptr kd_tree_ptr; }; diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index a4f13a4a..bdfc0426 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,13 @@ #include #include +// Half Edge Mesh (HEM) Base +#include + +// Half Edge Mesh (HEM) Implementations +#include +#include + #include #include #include @@ -71,6 +79,22 @@ namespace fs = std::filesystem; namespace mesh_map { +std::shared_ptr > createHemByName(std::string hem_impl, lvr2::MeshBufferPtr mesh_buffer) +{ + if(hem_impl == "pmp") + { + return std::make_shared >(mesh_buffer); + } + else if(hem_impl == "lvr") + { + return std::make_shared >(mesh_buffer); + } + + std::stringstream error_msg; + error_msg << "'" << hem_impl << "' not known." << std::endl; + throw std::runtime_error(error_msg.str()); +} + using HDF5MeshIO = lvr2::Hdf5Build; MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) @@ -79,7 +103,6 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) , first_config(true) , map_loaded(false) , layer_loader("mesh_map", "mesh_map::AbstractLayer") - , mesh_ptr(new lvr2::HalfEdgeMesh()) { auto min_contour_size_desc = rcl_interfaces::msg::ParameterDescriptor{}; min_contour_size_desc.name = MESH_MAP_NAMESPACE + ".min_contour_size"; @@ -111,6 +134,8 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node) cost_limit_desc.floating_point_range.push_back(cost_limit_range); cost_limit = node->declare_parameter(MESH_MAP_NAMESPACE + ".cost_limit", 1.0); + hem_impl_ = node->declare_parameter(MESH_MAP_NAMESPACE + ".hem", "pmp"); + 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", ""); @@ -179,9 +204,9 @@ 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 as default: '" << mesh_working_part << "'"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Mesh Working Part is empty. Using mesh part as default: '" << mesh_working_part << "'"); } else { - RCLCPP_INFO_STREAM(node->get_logger(), "Using mesh working part from parameter: '" << mesh_working_part << "'"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Using mesh working part from parameter: '" << mesh_working_part << "'"); } if(fs::path(mesh_working_file).extension() != ".h5") @@ -191,7 +216,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 << "\"..."); + RCLCPP_DEBUG_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); @@ -200,7 +225,7 @@ bool MeshMap::readMap() if(mesh_file != mesh_working_file) { - RCLCPP_INFO_STREAM(node->get_logger(), "Initially loading \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Initially loading \"" << mesh_part << "\" from file \"" << mesh_file << "\"..."); std::cout << "Generate seperate working file..." << std::endl; lvr2::MeshBufferPtr mesh_buffer; @@ -230,7 +255,6 @@ bool MeshMap::readMap() RCLCPP_ERROR_STREAM(node->get_logger(), "Error while loading map: " << io.GetErrorString()); return false; } - RCLCPP_INFO_STREAM(node->get_logger(), "extractMeshByName"); mesh_buffer = extractMeshByName(ascene, mesh_part); } @@ -243,39 +267,39 @@ bool MeshMap::readMap() RCLCPP_INFO_STREAM(node->get_logger(), "Loaded mesh buffer: \n" << *mesh_buffer); // write - hdf5_mesh_io->save(mesh_working_part, mesh_buffer); } } } else { - RCLCPP_INFO_STREAM(node->get_logger(), "Connection to file exists already!"); + RCLCPP_DEBUG_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 << "'..."); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Start reading the mesh part '" << mesh_part << "' from the map file '" << mesh_file << "'..."); auto hdf5_mesh_input = std::make_shared(); hdf5_mesh_input->open(mesh_working_file); hdf5_mesh_input->setMeshName(mesh_working_part); lvr2::MeshBufferPtr mesh_buffer = hdf5_mesh_input->MeshIO::load(mesh_working_part); - RCLCPP_INFO_STREAM(node->get_logger(), "Convert buffer to HEM: \n" << *mesh_buffer); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Convert buffer to HEM: \n" << *mesh_buffer); - // auto mesh_opt = mesh_io_ptr->getMesh(); if(mesh_buffer) { - mesh_ptr = std::make_shared >(mesh_buffer); - RCLCPP_INFO_STREAM(node->get_logger(), "The mesh has been loaded successfully with " + RCLCPP_DEBUG_STREAM(node->get_logger(), "Creating mesh of type '" << hem_impl_ << "'"); + mesh_ptr = createHemByName(hem_impl_, mesh_buffer); + RCLCPP_INFO_STREAM(node->get_logger(), "The mesh of type '" << hem_impl_ << "' 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, NanoFlannMeshAdaptor(mesh_ptr), nanoflann::KDTreeSingleIndexAdaptorParams(10)); + adaptor_ptr = std::make_unique(mesh_ptr); + kd_tree_ptr = std::make_unique(3,*adaptor_ptr, nanoflann::KDTreeSingleIndexAdaptorParams(10)); kd_tree_ptr->buildIndex(); RCLCPP_INFO_STREAM(node->get_logger(), "The k-d tree has been build successfully!"); } else { RCLCPP_ERROR_STREAM(node->get_logger(), "Could not load the mesh '" << mesh_part << "' from the map file '" << mesh_file << "' "); + throw std::runtime_error("Could not load mesh"); return false; } @@ -297,7 +321,7 @@ bool MeshMap::readMap() else { RCLCPP_INFO_STREAM(node->get_logger(), "No face normals found in the given map file, computing them..."); - face_normals = lvr2::calcFaceNormals(*mesh_ptr); + face_normals = lvr2::calcFaceNormals(*mesh_ptr); // -> lvr2::DenseFaceMap RCLCPP_INFO_STREAM(node->get_logger(), "Computed " << face_normals.numValues() << " face normals."); if (mesh_io_ptr->addDenseAttributeMap(face_normals, "face_normals")) { @@ -309,6 +333,7 @@ bool MeshMap::readMap() } } + RCLCPP_INFO_STREAM(node->get_logger(), "Create 'vertex_normals'"); auto vertex_normals_opt = mesh_io_ptr->getDenseAttributeMap>("vertex_normals"); if (vertex_normals_opt) @@ -512,6 +537,12 @@ void MeshMap::combineVertexCosts(const rclcpp::Time& map_stamp) const float factor = 1.0; // TODO: carefully think about this const float norm_factor = factor / norm; + + if(norm <= 0.00001) + { + RCLCPP_ERROR_STREAM(node->get_logger(), "Layer \"" << layer.first << "\": ERROR - range between max and min value has to be >0."); + } + RCLCPP_INFO_STREAM(node->get_logger(), "Layer \"" << layer.first << "\" max value: " << max << " min value: " << min << " norm: " << norm << " factor: " << factor << " norm factor: " << norm_factor); @@ -1108,13 +1139,19 @@ lvr2::OptionalFaceHandle MeshMap::getContainingFace(Vector& position, const floa { auto search_result = searchContainingFace(position, max_dist); if(search_result) + { return std::get<0>(*search_result); + } return lvr2::OptionalFaceHandle(); } -boost::optional, - std::array>> MeshMap::searchContainingFace( - Vector& query_point, const float& max_dist) +boost::optional face handle + std::array, // -> closest face vertices (why no handles?) + std::array // -> barycentric coords on closest face + >> MeshMap::searchContainingFace( // inputs: + Vector& query_point, // -> query point + const float& max_dist) // -> maximum search radius around query point { if(auto vH_opt = getNearestVertexHandle(query_point)) { @@ -1154,6 +1191,12 @@ lvr2::OptionalVertexHandle MeshMap::getNearestVertexHandle(const Vector& pos) float querry_point[3] = {pos.x, pos.y, pos.z}; size_t ret_index; float out_dist_sqr; + + if(!kd_tree_ptr) + { + throw std::runtime_error("Tried to access kd tree which is not yet initialized"); + } + size_t num_results = kd_tree_ptr->knnSearch(&querry_point[0], 1, &ret_index, &out_dist_sqr); return num_results == 0 ? lvr2::OptionalVertexHandle() : lvr2::VertexHandle(ret_index); } diff --git a/mesh_map/src/util.cpp b/mesh_map/src/util.cpp index e04bd6c6..7a957826 100644 --- a/mesh_map/src/util.cpp +++ b/mesh_map/src/util.cpp @@ -99,9 +99,6 @@ lvr2::MeshBufferPtr extractMeshByName( const aiScene* ascene, std::string name) { - std::cout << "extractMeshByName" << std::endl; - RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "HELLOO"); - lvr2::MeshBufferPtr mesh; const aiNode* root_node = ascene->mRootNode; @@ -114,7 +111,7 @@ lvr2::MeshBufferPtr extractMeshByName( if(path_to_mesh.size() == 0) { - std::cout << "path empty!" << std::endl; + RCLCPP_WARN_STREAM(rclcpp::get_logger("mesh_map/util"), "path empty!"); return mesh; } @@ -122,7 +119,7 @@ lvr2::MeshBufferPtr extractMeshByName( for(size_t i=1; i Date: Thu, 12 Dec 2024 01:08:10 +0100 Subject: [PATCH 5/9] reenabled info stats for mesh loading --- mesh_map/src/mesh_map.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index bdfc0426..2c88f962 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/mesh_map/src/mesh_map.cpp @@ -199,6 +199,7 @@ bool MeshMap::readMap() { // default: mesh_working_file = mesh_file filename in this directory mesh_working_file = fs::path(mesh_file).replace_extension(".h5"); + RCLCPP_INFO_STREAM(node->get_logger(), "No mesh working file specified. Setting it to '" << mesh_working_file << "'"); } if(mesh_working_part == "") @@ -216,7 +217,7 @@ bool MeshMap::readMap() } // directly work on the input file - RCLCPP_DEBUG_STREAM(node->get_logger(), "Connect to \"" << mesh_working_part << "\" from file \"" << mesh_working_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); @@ -225,7 +226,7 @@ bool MeshMap::readMap() if(mesh_file != mesh_working_file) { - RCLCPP_DEBUG_STREAM(node->get_logger(), "Initially loading \"" << mesh_part << "\" from file \"" << mesh_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; @@ -268,6 +269,8 @@ bool MeshMap::readMap() // write hdf5_mesh_io->save(mesh_working_part, mesh_buffer); + } else { + RCLCPP_INFO_STREAM(node->get_logger(), "Working mesh == input mesh"); } } } else { From d0d39cd0dd6bd893881036d80c009ac856e52a6f Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 20 Dec 2024 10:26:25 +0100 Subject: [PATCH 6/9] added lvr update branch to CI --- source_dependencies.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source_dependencies.yaml b/source_dependencies.yaml index 89938dfc..3e762016 100644 --- a/source_dependencies.yaml +++ b/source_dependencies.yaml @@ -4,7 +4,7 @@ repositories: lvr2: type: git url: https://github.com/uos/lvr2.git - version: humble + version: version-update mesh_tools: type: git url: https://github.com/naturerobots/mesh_tools.git From b3c77c67c8bc49c468181f8c5470200e8ff8e138 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 20 Dec 2024 10:29:46 +0100 Subject: [PATCH 7/9] added lvr update branch to CI --- source_dependencies.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source_dependencies.yaml b/source_dependencies.yaml index 3e762016..b8cb4f90 100644 --- a/source_dependencies.yaml +++ b/source_dependencies.yaml @@ -4,7 +4,7 @@ repositories: lvr2: type: git url: https://github.com/uos/lvr2.git - version: version-update + version: version-upgrade mesh_tools: type: git url: https://github.com/naturerobots/mesh_tools.git From 22a568d4c30d2a5e1bae764301db4980d12795a4 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 20 Dec 2024 10:57:45 +0100 Subject: [PATCH 8/9] changed mesh tools branch in source dependencies --- source_dependencies.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source_dependencies.yaml b/source_dependencies.yaml index b8cb4f90..40123961 100644 --- a/source_dependencies.yaml +++ b/source_dependencies.yaml @@ -8,7 +8,7 @@ repositories: mesh_tools: type: git url: https://github.com/naturerobots/mesh_tools.git - version: humble + version: lvr-update move_base_flex: type: git url: https://github.com/naturerobots/move_base_flex.git From 3f2e411e6aa25a35b89ae5cb1ca79423b0ec37c1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 23 Dec 2024 12:03:46 +0100 Subject: [PATCH 9/9] removed debug logs --- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 44b61969..cc416dac 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -651,25 +651,16 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto& vertex_costs = mesh_map_->vertexCosts(); auto& invalid = mesh_map_->invalid; - - RCLCPP_DEBUG_STREAM(node_->get_logger(), "BLA."); - mesh_map_->publishDebugPoint(original_start, mesh_map::color(0, 1, 0), "start_point"); mesh_map_->publishDebugPoint(original_goal, mesh_map::color(0, 0, 1), "goal_point"); - RCLCPP_DEBUG_STREAM(node_->get_logger(), "published point."); - mesh_map::Vector start = original_start; mesh_map::Vector goal = original_goal; - RCLCPP_DEBUG_STREAM(node_->get_logger(), "Find face."); - // Find the containing faces of start and goal const lvr2::OptionalFaceHandle start_opt = mesh_map_->getContainingFace(start, 0.4); const lvr2::OptionalFaceHandle goal_opt = mesh_map_->getContainingFace(goal, 0.4); - RCLCPP_DEBUG_STREAM(node_->get_logger(), "BLA 2."); - const auto t_initialization_start = std::chrono::steady_clock::now(); // reset cancel planning @@ -741,8 +732,6 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto t_wavefront_start = std::chrono::steady_clock::now(); const auto initialization_duration_ms = std::chrono::duration_cast(t_wavefront_start - t_initialization_start); - - RCLCPP_DEBUG_STREAM(node_->get_logger(), "GO."); while (!pq.isEmpty() && !cancel_planning_) { lvr2::VertexHandle current_vh = pq.popMin().key();