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,11 @@ int main(int argc, char** argv)
 
   signal(SIGINT, sigintHandler);
 
-  rclcpp::spin(node);
+  // NOTE: Using a multithreaded executor is recommended so cost layers can use
+  // different callback groups when processing sensor data.
+  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

@@ -98,26 +98,18 @@ class BorderLayer : public mesh_map::AbstractLayer
    *
    * @return calculated costmap
    */
-  virtual lvr2::VertexMap<float>& costs() override;
+  virtual const lvr2::VertexMap<float>& costs() override;
 
   /**
    * @brief deliver set containing all vertices marked as lethal
    *
    * @return lethal vertices
    */
-  virtual std::set<lvr2::VertexHandle>& lethals() override
+  virtual const std::set<lvr2::VertexHandle>& lethals() override
   {
     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

@@ -91,26 +91,18 @@ class ClearanceLayer : public mesh_map::AbstractLayer
    *
    * @return calculated costmap
    */
-  virtual lvr2::VertexMap<float>& costs() override;
+  virtual const lvr2::VertexMap<float>& costs() override;
 
   /**
    * @brief deliver set containing all vertices marked as lethal
    *
    * @return lethal vertices
    */
-  virtual std::set<lvr2::VertexHandle>& lethals() override
+  virtual const std::set<lvr2::VertexHandle>& lethals() override
   {
     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;}
+  const lvr2::VertexMap<float>& costs() override {return costs_;}
+  const 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

@@ -98,26 +98,18 @@ class HeightDiffLayer : public mesh_map::AbstractLayer
    *
    * @return calculated costmap
    */
-  virtual lvr2::VertexMap<float>& costs() override;
+  virtual const lvr2::VertexMap<float>& costs() override;
 
   /**
    * @brief deliver set containing all vertices marked as lethal
    *
    * @return lethal vertices
    */
-  virtual std::set<lvr2::VertexHandle>& lethals() override
+  virtual const std::set<lvr2::VertexHandle>& lethals() override
   {
     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
    *