Graph-Based Layer System for Dynamic Cost Updates

mbf_mesh_nav/src/mbf_mesh_nav.cpp

@@ -39,6 +39,7 @@
 #include <signal.h>
 #include <tf2_ros/transform_listener.h>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
 
 mbf_mesh_nav::MeshNavigationServer::Ptr mesh_nav_srv_ptr;
 rclcpp::Node::SharedPtr node;
@@ -66,7 +67,9 @@ int main(int argc, char** argv)
 
   signal(SIGINT, sigintHandler);
 
-  rclcpp::spin(node);
+  rclcpp::executors::MultiThreadedExecutor exec;
+  exec.add_node(node);
+  exec.spin();
 
   return EXIT_SUCCESS;
 }

mesh_layers/CMakeLists.txt

@@ -11,6 +11,7 @@ endif()
 find_package(ament_cmake_ros REQUIRED)
 find_package(mesh_map REQUIRED)
 find_package(LVR2 REQUIRED)
+find_package(OpenMP)
 add_definitions(${LVR2_DEFINITIONS})
 
 pluginlib_export_plugin_description_file(mesh_map mesh_layers.xml)
@@ -23,6 +24,7 @@ add_library(${PROJECT_NAME}
   src/steepness_layer.cpp
   src/ridge_layer.cpp
   src/clearance_layer.cpp
+  src/combination_layer.cpp
 )
 include_directories(
   include
@@ -38,6 +40,7 @@ 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}
+  OpenMP::OpenMP_CXX
 )
 
 install(DIRECTORY include/
@@ -51,6 +54,18 @@ install(TARGETS
   RUNTIME DESTINATION bin
 )
 
+if (BUILD_TESTING)
+  # Add GTest tests
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}_inflation_layer_test test/inflation_layer_test.cpp)
+  target_include_directories(${PROJECT_NAME}_inflation_layer_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  target_link_libraries(${PROJECT_NAME}_inflation_layer_test ${PROJECT_NAME})
+endif()
+
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(mesh_map)

mesh_layers/include/mesh_layers/border_layer.h

@@ -110,14 +110,6 @@ class BorderLayer : public mesh_map::AbstractLayer
     return lethal_vertices_;
   }
 
-  /**
-   * @brief update set of lethal vertices by adding and removing vertices
-   *
-   * @param added_lethal vertices to be marked as lethal
-   * @param removed_lethal vertices to be removed from the set of lethal vertices
-   */
-  virtual void updateLethal(std::set<lvr2::VertexHandle>& added_lethal, std::set<lvr2::VertexHandle>& removed_lethal) override {};
-
   /**
    * @brief initializes this layer plugin
    *

mesh_layers/include/mesh_layers/clearance_layer.h

@@ -103,14 +103,6 @@ class ClearanceLayer : public mesh_map::AbstractLayer
     return lethal_vertices_;
   }
 
-  /**
-   * @brief update set of lethal vertices by adding and removing vertices
-   *
-   * @param added_lethal vertices to be marked as lethal
-   * @param removed_lethal vertices to be removed from the set of lethal vertices
-   */
-  virtual void updateLethal(std::set<lvr2::VertexHandle>& added_lethal, std::set<lvr2::VertexHandle>& removed_lethal) override {};
-
   /**
    * @brief initializes this layer plugin
    *

mesh_layers/include/mesh_layers/combination_layer.h

@@ -0,0 +1,128 @@
+/*
+ *  Copyright 2025, Justus Braun
+ *
+ *  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:
+ *    Justus Braun <jubraun@uni-osnabrueck.de>
+ *
+ */
+#pragma once
+
+#include <mesh_map/abstract_layer.h>
+#include <rclcpp/rclcpp.hpp>
+
+namespace mesh_layers
+{
+
+class MaxCombinationLayer
+: public mesh_map::AbstractLayer
+{
+public:
+
+  bool readLayer() override {return false;}
+  bool writeLayer() override {return true;}
+  float defaultValue() override {return 0.0;}
+  float threshold() override {return 1.0;}
+  lvr2::VertexMap<float>& costs() override {return costs_;}
+  std::set<lvr2::VertexHandle>& lethals() override {return lethals_;}
+
+  /**
+  * @brief Combines the configured layers
+  */
+  bool computeLayer() override;
+
+  /**
+  * @brief Process a change in a lower layer
+  */
+  virtual void onInputChanged(
+    const rclcpp::Time& timestamp,
+    const std::set<lvr2::VertexHandle>& changed
+  ) override;
+
+  /**
+  * @brief Initialize the layer
+  */
+  bool initialize() override;
+
+private:
+  // Combined costs
+  lvr2::DenseVertexMap<float> costs_;
+
+  // Combined lethal vertices
+  std::set<lvr2::VertexHandle> lethals_;
+
+  // Configured layers; use weak_ptr to prevent shared_ptr cycles
+  std::vector<std::weak_ptr<mesh_map::AbstractLayer>> inputs_;
+};
+
+
+class CombinationLayer
+: public mesh_map::AbstractLayer
+{
+public:
+
+  bool readLayer() override {return false;}
+  bool writeLayer() override {return true;}
+  float defaultValue() override {return 0.0;}
+  float threshold() override {return 1.0;}
+  lvr2::VertexMap<float>& costs() override {return costs_;}
+  std::set<lvr2::VertexHandle>& lethals() override {return lethals_;}
+
+  /**
+  * @brief Combines the configured layers
+  */
+  bool computeLayer() override;
+
+  /**
+  * @brief Process a change in a lower layer
+  */
+  virtual void onInputChanged(
+    const rclcpp::Time& timestamp,
+    const std::set<lvr2::VertexHandle>& changed
+  ) override;
+
+  /**
+  * @brief Initialize the layer
+  */
+  bool initialize() override;
+
+private:
+  // Combined costs
+  lvr2::DenseVertexMap<float> costs_;
+
+  // Combined lethal vertices
+  std::set<lvr2::VertexHandle> lethals_;
+
+  // Configured layers; use weak_ptr to prevent shared_ptr cycles
+  std::vector<std::weak_ptr<mesh_map::AbstractLayer>> inputs_;
+};
+
+} // namespace mesh_map

mesh_layers/include/mesh_layers/height_diff_layer.h

@@ -110,14 +110,6 @@ class HeightDiffLayer : public mesh_map::AbstractLayer
     return lethal_vertices_;
   }
 
-  /**
-   * @brief update set of lethal vertices by adding and removing vertices
-   *
-   * @param added_lethal vertices to be marked as lethal
-   * @param removed_lethal vertices to be removed from the set of lethal vertices
-   */
-  virtual void updateLethal(std::set<lvr2::VertexHandle>& added_lethal, std::set<lvr2::VertexHandle>& removed_lethal) override {};
-
   /**
    * @brief initializes this layer plugin
    *

mesh_layers/include/mesh_layers/inflation_layer.h

@@ -39,6 +39,7 @@
 #define MESH_MAP__INFLATION_LAYER_H
 
 #include <mesh_map/abstract_layer.h>
+#include <lvr2/geometry/PMPMesh.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace mesh_layers
@@ -50,6 +51,7 @@ const float EPSILON = 1e-9;
  */
 class InflationLayer : public mesh_map::AbstractLayer
 {
+public:
   /**
    * @brief try read layer from map file
    *
@@ -81,13 +83,17 @@ class InflationLayer : public mesh_map::AbstractLayer
    */
   virtual float threshold() override;
 
-  inline float computeUpdateSethianMethod(const float& d1, const float& d2, const float& a, const float& b,
-                                          const float& dot, const float& F);
+  inline float computeUpdateSethianMethod(
+    const float& d1, const float& d2,
+    const float& a, const float& b,
+    const float& dot, const float& F
+  ) const;
 
   /**
    * @brief updates the wavefront
    *
    * @param distances current distances from the start vertices
+   * @param vector_map buffer to store new vectors towards the source in
    * @param predecessors current predecessors of vertices visited during the wave front propagation
    * @param max_distance max distance of propagation
    * @param edge_weights weights of the edges
@@ -99,11 +105,14 @@ class InflationLayer : public mesh_map::AbstractLayer
    *
    * @return true if successful; else false
    */
-  inline bool waveFrontUpdate(lvr2::DenseVertexMap<float>& distances,
-                              lvr2::DenseVertexMap<lvr2::VertexHandle>& predecessors, const float& max_distance,
-                              const lvr2::DenseEdgeMap<float>& edge_weights, const lvr2::FaceHandle& fh,
-                              const lvr2::BaseVector<float>& normal, const lvr2::VertexHandle& v1,
-                              const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3);
+  bool waveFrontUpdate(
+    const lvr2::PMPMesh<mesh_map::Vector>& mesh,
+    lvr2::DenseVertexMap<float>& distances,
+    lvr2::DenseVertexMap<mesh_map::Vector>& vector_map,
+    const float& max_distance,
+    const lvr2::DenseEdgeMap<float>& edge_weights,
+    const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3
+  ) const;
 
   /**
    * @brief fade cost value based on lethal and inscribed area
@@ -118,13 +127,22 @@ class InflationLayer : public mesh_map::AbstractLayer
    * @brief inflate around lethal vertices by using an wave front propagation and assign riskiness values to vertices
    *
    * @param lethals set of current lethal vertices
+   * @param[out] costs buffer to write the costs to
+   * @param[out] vectors buffer to write the vectors to
    * @param inflation_radius radius of inflation
    * @param inscribed_radius rarius of inscribed area
    * @param inscribed_value value assigned to inscribed vertices
    * @param lethal_value value of lethal vertices
    */
-  void waveCostInflation(const std::set<lvr2::VertexHandle>& lethals, const float inflation_radius,
-                         const float inscribed_radius, const float inscribed_value, const float lethal_value);
+  void waveCostInflation(
+    const std::set<lvr2::VertexHandle>& lethals,
+    lvr2::DenseVertexMap<float>& costs,
+    lvr2::DenseVertexMap<mesh_map::Vector>& vectors,
+    const float inflation_radius,
+    const float inscribed_radius,
+    const float inscribed_value,
+    const float lethal_value
+  );
 
   /**
    * @brief returns repulsive vector at a given position inside a face
@@ -134,7 +152,7 @@ class InflationLayer : public mesh_map::AbstractLayer
    *
    * @return vector of the current vectorfield
    */
-  lvr2::BaseVector<float> vectorAt(const std::array<lvr2::VertexHandle, 3>& vertices,
+  mesh_map::Vector vectorAt(const std::array<lvr2::VertexHandle, 3>& vertices,
                                    const std::array<float, 3>& barycentric_coords);
 
   /**
@@ -144,14 +162,14 @@ class InflationLayer : public mesh_map::AbstractLayer
    *
    * @return vector of vectorfield at requested vertex
    */
-  lvr2::BaseVector<float> vectorAt(const lvr2::VertexHandle& vertex);
+  mesh_map::Vector vectorAt(const lvr2::VertexHandle& vertex);
 
   /**
    * @brief delivers the current repulsive vectorfield
    *
    * @return repulsive vectorfield
    */
-  const boost::optional<lvr2::VertexMap<lvr2::BaseVector<float>>&> vectorMap()
+  const boost::optional<lvr2::VertexMap<mesh_map::Vector>&> vectorMap()
   {
     return vector_map_;
   }
@@ -165,7 +183,7 @@ class InflationLayer : public mesh_map::AbstractLayer
    */
   void backToSource(const lvr2::VertexHandle& current_vertex,
                     const lvr2::DenseVertexMap<lvr2::VertexHandle>& predecessors,
-                    lvr2::DenseVertexMap<lvr2::BaseVector<float>>& vector_map);
+                    lvr2::DenseVertexMap<mesh_map::Vector>& vector_map);
 
   /**
    * @brief calculate the values of this layer
@@ -191,13 +209,10 @@ class InflationLayer : public mesh_map::AbstractLayer
     return lethal_vertices_;
   }  // TODO remove... layer types
 
-  /**
-   * @brief update set of lethal vertices by adding and removing vertices
-   *
-   * @param added_lethal vertices to be marked as lethal
-   * @param removed_lethal vertices to be removed from the set of lethal vertices
-   */
-  virtual void updateLethal(std::set<lvr2::VertexHandle>& added_lethal, std::set<lvr2::VertexHandle>& removed_lethal);
+  virtual void onInputChanged(
+    const rclcpp::Time& timestamp,
+    const std::set<lvr2::VertexHandle>& changed
+  ) override;
 
   /**
    * @brief initializes this layer plugin
@@ -215,9 +230,7 @@ class InflationLayer : public mesh_map::AbstractLayer
 
   lvr2::DenseVertexMap<float> direction_;
 
-  lvr2::DenseVertexMap<lvr2::FaceHandle> cutting_faces_;
-
-  lvr2::DenseVertexMap<lvr2::BaseVector<float>> vector_map_;
+  lvr2::DenseVertexMap<mesh_map::Vector> vector_map_;
 
   lvr2::DenseVertexMap<float> distances_;
 

mesh_layers/include/mesh_layers/ridge_layer.h

@@ -111,14 +111,6 @@ class RidgeLayer : public mesh_map::AbstractLayer
     return lethal_vertices_;
   }
 
-  /**
-   * @brief update set of lethal vertices by adding and removing vertices
-   *
-   * @param added_lethal vertices to be marked as lethal
-   * @param removed_lethal vertices to be removed from the set of lethal vertices
-   */
-  virtual void updateLethal(std::set<lvr2::VertexHandle>& added_lethal, std::set<lvr2::VertexHandle>& removed_lethal) override {};
-
   /**
    * @brief initializes this layer plugin
    *