You can select the half-edge-mesh implementation via the parameter 'h…

…em' 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

@@ -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)

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<std::chrono::milliseconds>(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();

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)

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

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