Changes:

amock · amock · commit e28dfe3ffb05 · 2025-07-21T19:37:03.000+02:00
- Renamed 'factor' to 'combination_weight' and moved it to AbstractLayer
  since it is used by every layer (and the mesh map)
- Removed unused contour functions.
- Pulled moved computation of edge weight into seperate function
- Added comments
diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp
@@ -633,7 +633,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
     }
   }
 
-return false;
+  return false;
 }
 
 uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_start,
diff --git a/mesh_layers/include/mesh_layers/border_layer.h b/mesh_layers/include/mesh_layers/border_layer.h
@@ -139,7 +139,6 @@ class BorderLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.5;
     double border_cost = 1.0;
-    double factor = 1.0;
   } config_;
 };
 
diff --git a/mesh_layers/include/mesh_layers/clearance_layer.h b/mesh_layers/include/mesh_layers/clearance_layer.h
@@ -142,7 +142,6 @@ class ClearanceLayer : public mesh_map::AbstractLayer
   struct {
     double robot_height = 0.5;
     double height_inflation = 0.3;
-    double factor = 1.0;
   } config_;
 };
 
diff --git a/mesh_layers/include/mesh_layers/height_diff_layer.h b/mesh_layers/include/mesh_layers/height_diff_layer.h
@@ -139,7 +139,6 @@ class HeightDiffLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.185;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 
diff --git a/mesh_layers/include/mesh_layers/inflation_layer.h b/mesh_layers/include/mesh_layers/inflation_layer.h
@@ -227,7 +227,6 @@ class InflationLayer : public mesh_map::AbstractLayer
   struct {
     double inscribed_radius = 0.25;
     double inflation_radius = 0.4;
-    double factor = 1.0;
     double lethal_value = 1.0;
     double inscribed_value = 0.99;
     double cost_scaling_factor = 1.0;
diff --git a/mesh_layers/include/mesh_layers/ridge_layer.h b/mesh_layers/include/mesh_layers/ridge_layer.h
@@ -142,7 +142,6 @@ class RidgeLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.3;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 
diff --git a/mesh_layers/include/mesh_layers/roughness_layer.h b/mesh_layers/include/mesh_layers/roughness_layer.h
@@ -140,7 +140,6 @@ class RoughnessLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.3;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 
diff --git a/mesh_layers/include/mesh_layers/steepness_layer.h b/mesh_layers/include/mesh_layers/steepness_layer.h
@@ -138,7 +138,6 @@ class SteepnessLayer : public mesh_map::AbstractLayer
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   struct {
     double threshold = 0.3;
-    double factor = 1.0;
   } config_;
 
 };
diff --git a/mesh_layers/src/border_layer.cpp b/mesh_layers/src/border_layer.cpp
@@ -127,8 +127,6 @@ rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::v
       config_.border_cost = parameter.as_double();
       recompute_costs = true;
       recompute_lethals = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -175,16 +173,6 @@ bool BorderLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.border_cost = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost", config_.border_cost, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "Using this factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &BorderLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_layers/src/clearance_layer.cpp b/mesh_layers/src/clearance_layer.cpp
@@ -176,8 +176,6 @@ rcl_interfaces::msg::SetParametersResult ClearanceLayer::reconfigureCallback(std
     } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".height_inflation") {
       config_.height_inflation = parameter.as_double();
       needs_cost_recompute = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -214,19 +212,6 @@ bool ClearanceLayer::initialize()
       descriptor
     );
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "Using this factor to weight this layer.";
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(
-      mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor",
-      config_.factor,
-      descriptor
-    );
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &ClearanceLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_layers/src/height_diff_layer.cpp b/mesh_layers/src/height_diff_layer.cpp
@@ -122,8 +122,6 @@ rcl_interfaces::msg::SetParametersResult HeightDiffLayer::reconfigureCallback(st
     } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") {
       config_.radius = parameter.as_double();
       recompute_lethals = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -159,16 +157,6 @@ bool HeightDiffLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "Using this factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &HeightDiffLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_layers/src/inflation_layer.cpp b/mesh_layers/src/inflation_layer.cpp
@@ -595,16 +595,6 @@ bool InflationLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "The factor to weight this layer";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   { // lethal_value
     rcl_interfaces::msg::ParameterDescriptor descriptor;
     descriptor.description = "Defines the 'lethal' value for obstacles. -1 results in infinity";
diff --git a/mesh_layers/src/ridge_layer.cpp b/mesh_layers/src/ridge_layer.cpp
@@ -202,8 +202,6 @@ rcl_interfaces::msg::SetParametersResult RidgeLayer::reconfigureCallback(std::ve
     } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") {
       config_.radius = parameter.as_double();
       has_radius_changed = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -240,16 +238,6 @@ bool RidgeLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "The local ridge factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &RidgeLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_layers/src/roughness_layer.cpp b/mesh_layers/src/roughness_layer.cpp
@@ -156,8 +156,6 @@ rcl_interfaces::msg::SetParametersResult RoughnessLayer::reconfigureCallback(std
       has_threshold_changed = true;
     } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") {
       config_.radius = parameter.as_double();
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -191,16 +189,6 @@ bool RoughnessLayer::initialize() {
     descriptor.floating_point_range.push_back(range);
     config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "The local roughness factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &RoughnessLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_layers/src/steepness_layer.cpp b/mesh_layers/src/steepness_layer.cpp
@@ -179,8 +179,6 @@ rcl_interfaces::msg::SetParametersResult SteepnessLayer::reconfigureCallback(std
     if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") {
       config_.threshold = parameter.as_double();
       has_threshold_changed = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -205,16 +203,6 @@ bool SteepnessLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "The local steepness factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &SteepnessLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;
diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt
@@ -37,6 +37,7 @@ include_directories(
 )
 
 add_library(${PROJECT_NAME}
+  src/abstract_layer.cpp
   src/mesh_map.cpp
   src/util.cpp
 )
diff --git a/mesh_map/include/mesh_map/abstract_layer.h b/mesh_map/include/mesh_map/abstract_layer.h
@@ -46,7 +46,6 @@
 
 #ifndef MESH_MAP__ABSTRACT_LAYER_H
 #define MESH_MAP__ABSTRACT_LAYER_H
-
 namespace mesh_map
 {
 class MeshMap;
@@ -148,42 +147,56 @@ class AbstractLayer
     return lvr2::BaseVector<float>();
   }
 
-  /**
-   * @brief Initializes the layer plugin with the given name.
-   */
-  virtual bool initialize() = 0;
-
   /**
    * @brief Initializes the layer plugin under the mesh_map namespace ans sets some basic attributes.
    */
-  virtual bool initialize(
+  bool initialize(
     const std::string& name,
     const notify_func notify_update,
     std::shared_ptr<mesh_map::MeshMap> map,
-    const rclcpp::Node::SharedPtr node)
-  {
-    layer_name_ = name;
-    node_ = node;
-    layer_namespace_ = "mesh_map/" + name;
-    notify_ = notify_update;
-    map_ptr_ = map;
-    return initialize();
-  }
+    const rclcpp::Node::SharedPtr node);
 
   void notifyChange()
   {
     this->notify_(layer_name_);
   }
 
+  inline float combinationWeight() const
+  {
+    return combination_weight_;
+  }
+
 protected:
+
+  /**
+   * @brief Initializes a custom layer plugin and is invoked at the end of the abstract layer's initialization.
+   */
+  virtual bool initialize() = 0;
+
   std::string layer_name_;
   std::shared_ptr<mesh_map::MeshMap> map_ptr_;
 
   rclcpp::Node::SharedPtr node_;
   std::string layer_namespace_;
 
+  //! Factor used to linearly combine layers to a combined layer in MeshMap
+  float combination_weight_ = 1.0;
+
 private:
   notify_func notify_;
+
+  /**
+   * @brief declares all ROS parameters
+   */
+  void declare_parameters();
+
+  /**
+   * @brief callback for incoming param changes
+   */
+  rcl_interfaces::msg::SetParametersResult reconfigureCallback(
+    const std::vector<rclcpp::Parameter>& parameters);
+
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
 } /* namespace mesh_map */
diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h
@@ -62,7 +62,6 @@
 #include "nanoflann.hpp"
 #include "nanoflann_mesh_adaptor.h"
 
-
 namespace mesh_map
 {
 class MeshMap : public std::enable_shared_from_this<MeshMap>
@@ -135,12 +134,19 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
    */
   void combineVertexCosts(const rclcpp::Time& map_stamp);
 
+  /**
+   * @brief pre-computes edge weights from combined vertex costs.
+   * The result can be directly used inside a search algorithm 
+   * that searches over the edges to a given target
+   */
+  void computeEdgeWeights();
+
   /**
    * @brief Computes contours
    * @param contours the vector to bo filled with contours
    * @param min_contour_size The minimum contour size, i.e. the number of vertices per contour.
    */
-  void findContours(std::vector<std::vector<lvr2::VertexHandle>>& contours, int min_contour_size);
+  // void findContours(std::vector<std::vector<lvr2::VertexHandle>>& contours, int min_contour_size);
 
   /**
    * @brief Publishes the given vertex map as mesh_msgs/VertexCosts, e.g. to visualize these.
@@ -188,14 +194,6 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
    */
   void layerChanged(const std::string& layer_name);
 
-  /**
-   * @brief Compute all contours and returns the corresponding vertices to use these as lethal vertices.
-   * @param min_contour_size The minimum contour size, i.e. the number of vertices per contour.
-   * @param min_contour_size
-   * @param lethals the vector which is filled with contour vertices
-   */
-  void findLethalByContours(const int& min_contour_size, std::set<lvr2::VertexHandle>& lethals);
-
   /**
    * @brief Returns the global frame / coordinate system id string
    */
@@ -484,8 +482,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr save_service;
 
   // Reconfigurable parameters (see reconfigureCallback method)
-  int min_contour_size;
-  double layer_factor;
+  double edge_cost_factor;
   double cost_limit;
 
   //! combined layer costs
diff --git a/mesh_map/src/abstract_layer.cpp b/mesh_map/src/abstract_layer.cpp
diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp

Original file line number	Diff line number	Diff line change
`@@ -633,7 +633,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(`
`633`	`633`	`}`
`634`	`634`	`}`
`635`	`635`
`636`		`-return false;`
	`636`	`+ return false;`
`637`	`637`	`}`
`638`	`638`
`639`	`639`	`uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_start,`