Dynamic Cost Updates

README.md

@@ -103,6 +103,7 @@ The package structure is as follows:
   - ClearanceLayer - `mesh_layers/ClearanceLayer`
   - InflationLayer - `mesh_layers/InflationLayer`
   - BorderLayer - `mesh_layers/BorderLayer`
+  - ObstacleLayer - `mesh_layers/ObstacleLayer`
 
 - `dijkstra_mesh_planner` contains a mesh planner plugin providing a path planning method based on Dijkstra's algorithm. It plans by using the edges of the mesh map. The propagation start a the goal pose, thus a path from every accessed vertex to the goal pose can be computed. This leads to a sub-optimal potential field, which highly depends on the mesh structure.
 
@@ -125,6 +126,7 @@ The following table gives an overview of all currently implemented layer plugins
 | **ClearanceLayer**  | `mesh_layers/ClearanceLayer`  | comparison of robot height and clearance along each vertex normal | ![ClearanceLayer](docs/images/costlayers/clearance.jpg?raw=true "Clearance Layer") |
 | **InflationLayer**  | `mesh_layers/InflationLayer`  | by distance to a lethal vertex           | ![InflationLayer](docs/images/costlayers/inflation.jpg?raw=true "Inflation Layer")      |
 | **BorderLayer** | `mesh_layers/BorderLayer` | give vertices close to the border a certain cost | ![BorderLayer](docs/images/costlayers/border.png?raw=true "Border Layer")   |
+| **ObstacleLayer**   | `mesh_layers/ObstacleLayer`   | marks vertices blocked by dynamic obstacles as lethal. Cost layer (left) and gazebo sim with unmapped obstacle (right) | ![ObstacleLayer](docs/images/costlayers/obstacle.png?raw=true "ObstacleLayer") |
 
 ## Planners
 Currently the following planners are available:

cvp_mesh_planner/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package cvp_mesh_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.0.0 (2025-09-10)
+------------------
+* Cost-aware planning
+
 2.0.1 (2025-08-11)
 ------------------
 * Bug fixes and cleanup since initial ROS 2 port

cvp_mesh_planner/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>cvp_mesh_planner</name>
-    <version>2.0.1</version>
+    <version>3.0.0</version>
     <description>The Continuous Vector Field Planner (CVP) mesh planner package</description>
     <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
     <maintainer email="sebastian.puetz@naturerobots.com">Sebastian Pütz</maintainer>

cvp_mesh_planner/src/cvp_mesh_planner.cpp

@@ -742,7 +742,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
     if (distances[current_vh] > goal_dist)
       continue;
 
-    if (vertex_costs[current_vh] > config_.cost_limit)
+    if (vertex_costs[current_vh] >= config_.cost_limit)
       continue;
 
     if (invalid[current_vh])
@@ -786,8 +786,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
         else if (fixed[a] && fixed[b] && !fixed[c])
         {
           // c is free
-          // Skip vertices above the cost limit
-          if (costs[c] > config_.cost_limit)
+          // Skip vertices at or above the cost limit
+          if (costs[c] >= config_.cost_limit)
           {
             continue;
           }
@@ -809,8 +809,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
         else if (fixed[a] && !fixed[b] && fixed[c])
         {
           // b is free
-          // Skip vertices above the cost limit
-          if (costs[b] > config_.cost_limit)
+          // Skip vertices at or above the cost limit
+          if (costs[b] >= config_.cost_limit)
           {
             continue;
           }
@@ -832,8 +832,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
         else if (!fixed[a] && fixed[b] && fixed[c])
         {
           // a if free
-          // Skip vertices above the cost limit
-          if (costs[a] > config_.cost_limit)
+          // Skip vertices at or above the cost limit
+          if (costs[a] >= config_.cost_limit)
           {
             continue;
           }

dijkstra_mesh_planner/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package dijkstra_mesh_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.0.0 (2025-09-10)
+------------------
+* Cost-aware planning
+
 2.0.1 (2025-08-11)
 ------------------
 * Bug fixes and cleanup since initial ROS 2 port

dijkstra_mesh_planner/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>dijkstra_mesh_planner</name>
-    <version>2.0.1</version>
+    <version>3.0.0</version>
     <description>The dijkstra_mesh_planner package</description>
     <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
     <maintainer email="sebastian.puetz@naturerobots.com">Sebastian Pütz</maintainer>

mbf_mesh_core/CHANGELOG.rst

@@ -1,6 +1,8 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package mbf_mesh_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+3.0.0 (2025-09-10)
+------------------
 
 2.0.1 (2025-08-11)
 ------------------

mbf_mesh_core/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>mbf_mesh_core</name>
-    <version>2.0.1</version>
+    <version>3.0.0</version>
     <description>The mbf_mesh_core package</description>
 
     <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>

mbf_mesh_nav/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package mbf_mesh_nav
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.0.0 (2025-09-10)
+------------------
+* Multi-threaded executor
+
 2.0.1 (2025-08-11)
 ------------------
 * Bug fixes and cleanup since initial ROS 2 port

mbf_mesh_nav/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>mbf_mesh_nav</name>
-    <version>2.0.1</version>
+    <version>3.0.0</version>
     <description>The mbf_mesh_nav package</description>
 
     <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>

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_controller/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package mesh_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.0.0 (2025-09-10)
+------------------
+
 2.0.1 (2025-08-11)
 ------------------
 * Bug fixes and cleanup since initial ROS 2 port

mesh_controller/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>mesh_controller</name>
-    <version>2.0.1</version>
+    <version>3.0.0</version>
     <description>The mesh_controller package</description>
 
     <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>

mesh_layers/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package mesh_layers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.0.0 (2025-09-10)
+------------------
+* Graph Layer System
+
 2.0.1 (2025-08-11)
 ------------------
 * Bug fixes and cleanup since initial ROS 2 port

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,8 @@ add_library(${PROJECT_NAME}
   src/steepness_layer.cpp
   src/ridge_layer.cpp
   src/clearance_layer.cpp
+  src/combination_layer.cpp
+  src/obstacle_layer.cpp
 )
 include_directories(
   include
@@ -38,6 +41,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 +55,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
    *