Merge pull request #90 from naturerobots/fix/cost-layers-get-lost-over-network-connections

amock · web-flow · commit 841c50c20372 · 2025-11-12T11:03:57.000+01:00
Move cost layer publishing responsibility to the layer manager
diff --git a/mesh_map/include/mesh_map/layer_manager.h b/mesh_map/include/mesh_map/layer_manager.h
@@ -47,6 +47,9 @@
 #include <mesh_map/abstract_layer.h>
 #include <mesh_map/definitions.h>
 
+#include <mesh_msgs/msg/mesh_vertex_costs_stamped.hpp>
+#include <mesh_msgs/msg/mesh_vertex_costs_sparse_stamped.hpp>
+
 namespace mesh_map
 {
 
@@ -55,9 +58,9 @@ class LayerManager
 public:
 
   /**
-   *  @brief Read the configured layers and dependencies from the node
+   *  @brief Read the configured layers and dependencies from the stored node
    */
-  void read_configured_layers(const rclcpp::Node::SharedPtr& node);
+  void read_configured_layers();
   
   /**
    *  @brief Create instances of the configured layers
@@ -69,7 +72,7 @@ class LayerManager
    *  @brief Initialize all layer plugins and load or compute their values
    *  @return true If all loaded layers were initialized sucessfully
    */
-  bool initialize_layer_plugins(const rclcpp::Node::SharedPtr& node, const std::shared_ptr<MeshMap>& map);
+  bool initialize_layer_plugins(const std::shared_ptr<MeshMap>& map);
   
   /**
    *  @brief Get the plugin type of a layer
@@ -109,6 +112,22 @@ class LayerManager
     return instances_;
   }
 
+  /**
+   * @brief Publishes the given vertex map as mesh_msgs/VertexCosts, e.g. to visualize these.
+   * @param costs The cost map to publish
+   * @param name The name of the cost map
+   * @param timestamp The timestamp of the cost layer
+   */
+  void publish_cost_layer(const lvr2::VertexMap<float>& costs, const std::string& name, const rclcpp::Time& timestamp);
+
+  /**
+   * @brief Publishes the given vertex map as mesh_msgs/VertexCostsSparse, e.g. to update the visualization in RVIZ.
+   * @param costs The cost map to publish
+   * @param name The name of the cost map
+   * @param timestamp The timestamp of the cost update
+   */
+  void publish_cost_update(const lvr2::VertexMap<float>& costs, const float default_value, const std::string& name, const rclcpp::Time& timestamp);
+
   /**
    *  @brief Init the layer manager. Stores a reference to the map, which is not used
    *  in the constructor!
@@ -162,6 +181,13 @@ class LayerManager
   //! Maps layer name to instance
   std::map<std::string, std::shared_ptr<AbstractLayer>> instances_;
 
+  //! Publisher for vertex costs
+  rclcpp::Publisher<mesh_msgs::msg::MeshVertexCostsStamped>::SharedPtr cost_pub_;
+  rclcpp::TimerBase::SharedPtr cost_subscribe_checker_;
+  size_t vertex_cost_sub_count_ = 0;
+
+  //! Publisher for vertex cost changes
+  rclcpp::Publisher<mesh_msgs::msg::MeshVertexCostsSparseStamped>::SharedPtr cost_update_pub_;
 };
 
 } /* namespace mesh_map */
diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h
@@ -143,16 +143,10 @@ class MeshMap
    * @brief Publishes the given vertex map as mesh_msgs/VertexCosts, e.g. to visualize these.
    * @param costs The cost map to publish
    * @param name The name of the cost map
+   * @param timestamp The timestamp of the costmap
    */
   void publishVertexCosts(const lvr2::VertexMap<float>& costs, const std::string& name, const rclcpp::Time& map_stamp);
 
-  /**
-   * @brief Publishes the given vertex map as mesh_msgs/VertexCostsSparse, e.g. to update the visualization in RVIZ.
-   * @param costs The cost map to publish
-   * @param name The name of the cost map
-   */
-  void publishVertexCostsUpdate(const lvr2::VertexMap<float>& costs, const float default_value, const std::string& name, const rclcpp::Time& map_stamp);
-
   /**
    * @briefP Publishes the vertex colors if these exists.
    */
@@ -529,12 +523,6 @@ class MeshMap
   //! vertex normals
   lvr2::DenseVertexMap<Normal> vertex_normals;
 
-  //! publisher for vertex costs
-  rclcpp::Publisher<mesh_msgs::msg::MeshVertexCostsStamped>::SharedPtr vertex_costs_pub;
-  rclcpp::TimerBase::SharedPtr vertex_costs_subscribe_checker_;
-  size_t vertex_costs_sub_count_ = 0;
-  rclcpp::Publisher<mesh_msgs::msg::MeshVertexCostsSparseStamped>::SharedPtr vertex_costs_update_pub_;
-
   //! publisher for vertex colors
   rclcpp::Publisher<mesh_msgs::msg::MeshVertexColorsStamped>::SharedPtr vertex_colors_pub;
 
diff --git a/mesh_map/src/layer_manager.cpp b/mesh_map/src/layer_manager.cpp
@@ -15,9 +15,9 @@ LayerManager::LayerManager(MeshMap& map, const rclcpp::Node::SharedPtr& node)
 , loader_("mesh_map", "mesh_map::AbstractLayer")
 {}
 
-void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
+void LayerManager::read_configured_layers()
 {
-  const auto layer_names = node->declare_parameter(
+  const auto layer_names = node_->declare_parameter(
     MeshMap::MESH_MAP_NAMESPACE + ".layers", std::vector<std::string>()
   );
   const rclcpp::ParameterType ros_param_type = rclcpp::ParameterType::PARAMETER_STRING;
@@ -29,7 +29,7 @@ void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
       throw rclcpp::exceptions::InvalidParametersException("The layer name " + layer_name + " is used more than once. Layer names must be unique!");
     }
     // This will throws rclcpp::ParameterValue exception if mesh_map.layer_name.type is not set
-    const std::string layer_type = node->declare_parameter<std::string>(
+    const std::string layer_type = node_->declare_parameter<std::string>(
       MeshMap::MESH_MAP_NAMESPACE + "." + layer_name + ".type"
     );
 
@@ -42,7 +42,7 @@ void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
   // output warning if no layer plugins were configured
   if (names_.size() == 0)
   {
-    RCLCPP_WARN_STREAM(node->get_logger(), "No MeshMap layer plugins configured!"
+    RCLCPP_WARN_STREAM(node_->get_logger(), "No MeshMap layer plugins configured!"
       << " - Use the param \"" << MeshMap::MESH_MAP_NAMESPACE << ".layers\", which must be a list of strings with arbitrary layer names. "
       << "For each layer_name, also define layer_name.type with the respective type that shall be loaded via pluginlib.");
   }
@@ -59,7 +59,7 @@ void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
   {
     const Vertex& v = vertices_[layer_name];
     // If mesh_map.layer_name.inputs is not set we assume the layer needs no inputs
-    const auto dependencies = node->declare_parameter<std::vector<std::string>>(
+    const auto dependencies = node_->declare_parameter<std::vector<std::string>>(
       MeshMap::MESH_MAP_NAMESPACE + "." + layer_name + ".inputs", std::vector<std::string>()
     );
     
@@ -72,7 +72,7 @@ void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
   }
   
   // Print the dependencies
-  RCLCPP_INFO(node->get_logger(), "Layer dependencies:");
+  RCLCPP_INFO(node_->get_logger(), "Layer dependencies:");
   for (const auto& [name, vertex]: vertices_)
   {
     auto [it, end] = boost::adjacent_vertices(vertex, graph_);
@@ -90,8 +90,28 @@ void LayerManager::read_configured_layers(const rclcpp::Node::SharedPtr& node)
     }
     builder << ']';
 
-    RCLCPP_INFO(node->get_logger(), "  %s: %s", name.c_str(), builder.str().c_str());
+    RCLCPP_INFO(node_->get_logger(), "  %s: %s", name.c_str(), builder.str().c_str());
   }
+
+  // Initialize the publishers
+  cost_pub_ = node_->create_publisher<mesh_msgs::msg::MeshVertexCostsStamped>("~/vertex_costs", rclcpp::QoS(names_.size()).transient_local());
+
+  // we dont want to publish static costs all the time.
+  // therefore we have to check if someone newly subscribed to the topic
+  cost_subscribe_checker_ = node_->create_wall_timer(std::chrono::milliseconds(200), [this]{
+      const size_t count = cost_pub_->get_subscription_count() + cost_pub_->get_intra_process_subscription_count();
+      if (count > vertex_cost_sub_count_)
+      {
+        RCLCPP_INFO(node_->get_logger(), "New cost layer subscriber detected. Publishing vertex costs once...");
+        for (const auto& [layer_name, layer_ptr]: layer_instances())
+        {
+          publish_cost_layer(layer_ptr->costs(), layer_name, node_->get_clock()->now());
+        }
+      }
+      vertex_cost_sub_count_ = count;
+    });
+
+  cost_update_pub_ = node_->create_publisher<mesh_msgs::msg::MeshVertexCostsSparseStamped>(std::string(cost_pub_->get_topic_name()) + "/updates", rclcpp::QoS(10).transient_local());
 }
 
 bool LayerManager::load_layer_plugins(const rclcpp::Logger& logger)
@@ -125,7 +145,7 @@ bool LayerManager::load_layer_plugins(const rclcpp::Logger& logger)
   return instances_.empty() ? false : true;
 }
 
-bool LayerManager::initialize_layer_plugins(const rclcpp::Node::SharedPtr& node, const MeshMap::Ptr& map)
+bool LayerManager::initialize_layer_plugins(const MeshMap::Ptr& map)
 {
   // Prevent layers from publishing updates while initialization is still ongoing initialization
   std::unique_lock lock(layer_mtx_);
@@ -142,7 +162,7 @@ bool LayerManager::initialize_layer_plugins(const rclcpp::Node::SharedPtr& node,
     if (nullptr == instance)
     {
       RCLCPP_WARN(
-        node->get_logger(),
+        node_->get_logger(),
         "Skipping initialization of layer '%s' because it failed to load",
         name.c_str()
       );
@@ -156,10 +176,10 @@ bool LayerManager::initialize_layer_plugins(const rclcpp::Node::SharedPtr& node,
       std::placeholders::_3
     );
 
-    if (!instance->initialize(name, callback, map, node))
+    if (!instance->initialize(name, callback, map, node_))
     {
       RCLCPP_ERROR(
-        node->get_logger(),
+        node_->get_logger(),
         "Could not initialize the layer plugin with the name \"%s\"!",
         name.c_str()
       );
@@ -168,12 +188,12 @@ bool LayerManager::initialize_layer_plugins(const rclcpp::Node::SharedPtr& node,
 
     if (!instance->readLayer())
     {
-      RCLCPP_INFO(node->get_logger(), "Computing layer '%s' ...", name.c_str());
+      RCLCPP_INFO(node_->get_logger(), "Computing layer '%s' ...", name.c_str());
       instance->computeLayer();
     }
 
     // Publish the layer
-    map_.publishVertexCosts(instance->costs(), name, node_->get_clock()->now());
+    publish_cost_layer(instance->costs(), name, node_->get_clock()->now());
   }
 
   return true;
@@ -199,14 +219,14 @@ void LayerManager::layer_changed(
   const auto& ptr = get_layer(name);
   if (nullptr == ptr)
   {
-    // TODO: Log error
+    RCLCPP_ERROR(node_->get_logger(), "LayerManager::layer_changed() - Could not get pointer to changed layer %s", name.c_str());
     return;
   }
 
   const auto v_it = vertices_.find(name);
   if (v_it == vertices_.end())
   {
-    // TODO: Log error
+    RCLCPP_ERROR(node_->get_logger(), "LayerManager::layer_changed() - Could not find dependency graph vertex for layer %s", name.c_str());
     return;
   }
   
@@ -221,7 +241,7 @@ void LayerManager::layer_changed(
     c.insert(v, cm.containsKey(v)? cm[v]: ptr->defaultValue());
   }
   rlock.unlock();
-  map_.publishVertexCostsUpdate(c, ptr->defaultValue(), name, timestamp);
+  publish_cost_update(c, ptr->defaultValue(), name, timestamp);
   
   // Notify the map
   map_.layerChanged(name, changed);
@@ -242,4 +262,33 @@ void LayerManager::layer_changed(
   }
 }
 
+void LayerManager::publish_cost_layer(const lvr2::VertexMap<float>& costs, const std::string& name, const rclcpp::Time& timestamp)
+{
+  cost_pub_->publish(
+    mesh_msgs_conversions::toVertexCostsStamped(
+      costs,
+      map_.mesh()->numVertices(),
+      0,
+      name,
+      map_.getGlobalFrameID(),
+      map_.getUUID(),
+      timestamp
+    )
+  );
+}
+
+void LayerManager::publish_cost_update(const lvr2::VertexMap<float>& costs, const float default_value, const std::string& name, const rclcpp::Time& timestamp)
+{
+  cost_update_pub_->publish(
+    mesh_msgs_conversions::toVertexCostsSparseStamped(
+      costs,
+      default_value,
+      name,
+      map_.getGlobalFrameID(),
+      map_.getUUID(),
+      timestamp
+    )
+  );
+}
+
 } // namespace mesh_map;
diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp
@@ -130,19 +130,7 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node)
 
   marker_pub = node->create_publisher<visualization_msgs::msg::Marker>("~/marker", 100);
   mesh_geometry_pub = node->create_publisher<mesh_msgs::msg::MeshGeometryStamped>("~/mesh", rclcpp::QoS(1).transient_local());
-  vertex_costs_pub = node->create_publisher<mesh_msgs::msg::MeshVertexCostsStamped>("~/vertex_costs", rclcpp::QoS(1).transient_local());
-  // we dont want to publish static costs all the time.
-  // therefore we have to check if someone newly subscribed to the topic
-  vertex_costs_subscribe_checker_ = node->create_wall_timer(std::chrono::milliseconds(200), [this, node]{
-      const size_t count = vertex_costs_pub->get_subscription_count() + vertex_costs_pub->get_intra_process_subscription_count();
-      if (count > vertex_costs_sub_count_)
-      {
-        RCLCPP_INFO(node->get_logger(), "New cost layer subscriber detected. Publishing vertex costs once...");
-        publishCostLayers(node->now());
-      }
-      vertex_costs_sub_count_ = count;
-    });
-  vertex_costs_update_pub_ = node->create_publisher<mesh_msgs::msg::MeshVertexCostsSparseStamped>(std::string(vertex_costs_pub->get_topic_name()) + "/updates", rclcpp::QoS(10).transient_local());
+
   vertex_colors_pub = node->create_publisher<mesh_msgs::msg::MeshVertexColorsStamped>("~/vertex_colors", rclcpp::QoS(1).transient_local());
   vector_field_pub = node->create_publisher<visualization_msgs::msg::Marker>("~/vector_field", rclcpp::QoS(1).transient_local());
   edge_weights_text_pub = node->create_publisher<visualization_msgs::msg::MarkerArray>("~/edge_weights", rclcpp::QoS(1).transient_local());
@@ -434,7 +422,7 @@ bool MeshMap::readMap()
     }
   }
   
-  layer_manager_.read_configured_layers(node);
+  layer_manager_.read_configured_layers();
 
   RCLCPP_INFO_STREAM(node->get_logger(), "Load layer plugins...");
   if (!layer_manager_.load_layer_plugins(node->get_logger()))
@@ -444,7 +432,7 @@ bool MeshMap::readMap()
   }
 
   RCLCPP_INFO_STREAM(node->get_logger(), "Initialize layer plugins...");
-  if (!layer_manager_.initialize_layer_plugins(node, shared_from_this()))
+  if (!layer_manager_.initialize_layer_plugins(shared_from_this()))
   {
     RCLCPP_FATAL_STREAM(node->get_logger(), "Could not initialize plugins!");
     return false;
@@ -456,7 +444,6 @@ bool MeshMap::readMap()
     return false;
   }
   computeEdgeWeights();
-  publishCostLayers(map_stamp);
 
   map_loaded = true;
   return true;
@@ -1320,44 +1307,9 @@ bool MeshMap::resetLayers()
   return true;  // TODO implement
 }
 
-void MeshMap::publishCostLayers(const rclcpp::Time& map_stamp)
-{
-  for (const auto& [layer_name, layer_ptr] : layer_manager_.layer_instances())
-  {
-    vertex_costs_pub->publish(
-      mesh_msgs_conversions::toVertexCostsStamped(
-        layer_ptr->costs(),
-        mesh_ptr->numVertices(),
-        layer_ptr->defaultValue(),
-        layer_name,
-        global_frame,
-        uuid_str,
-        map_stamp
-      )
-    );
-  }
-}
-
 void MeshMap::publishVertexCosts(const lvr2::VertexMap<float>& costs, const std::string& name, const rclcpp::Time& map_stamp)
 {
-  vertex_costs_pub->publish(
-      mesh_msgs_conversions::toVertexCostsStamped(costs, mesh_ptr->numVertices(), 0, name, global_frame, uuid_str, map_stamp));
-  // TODO: Should this be moved to the LayerManager? Publishing the layers and updates
-  // is part of managing the layers isnt it?
-}
-
-void MeshMap::publishVertexCostsUpdate(const lvr2::VertexMap<float>& costs, const float default_value, const std::string& name, const rclcpp::Time& map_stamp)
-{
-  vertex_costs_update_pub_->publish(
-    mesh_msgs_conversions::toVertexCostsSparseStamped(
-      costs,
-      default_value,
-      name,
-      global_frame,
-      uuid_str,
-      map_stamp
-    )
-  );
+  layer_manager_.publish_cost_layer(costs, name, map_stamp);
 }
 
 void MeshMap::publishVertexColors(const rclcpp::Time& map_stamp)
diff --git a/mesh_map/test/mesh_map_test.cpp b/mesh_map/test/mesh_map_test.cpp
@@ -44,7 +44,7 @@ TEST_F(MeshMapTest, loadsSinglePlugin)
   // The MeshMap requires that a map file is loaded to initialize, so we test the layer manager directly
   mesh_map::LayerManager manager(*mesh_map_ptr_, node_ptr_);
 
-  EXPECT_NO_THROW(manager.read_configured_layers(node_ptr_));
+  EXPECT_NO_THROW(manager.read_configured_layers());
   EXPECT_TRUE(manager.load_layer_plugins(node_ptr_->get_logger()));
   // This calls back to the mesh map and segfaults because the MeshMap has its own internal LayerManager :(
   // EXPECT_TRUE(manager.initialize_layer_plugins(node_ptr_, mesh_map_ptr_));
@@ -65,7 +65,7 @@ TEST_F(MeshMapTest, loadsMultiplePlugins)
   // The MeshMap requires that a map file is loaded to initialize, so we test the layer manager directly
   mesh_map::LayerManager manager(*mesh_map_ptr_, node_ptr_);
 
-  EXPECT_NO_THROW(manager.read_configured_layers(node_ptr_));
+  EXPECT_NO_THROW(manager.read_configured_layers());
   EXPECT_TRUE(manager.load_layer_plugins(node_ptr_->get_logger()));
 
   EXPECT_NE(manager.get_layer("t1"), nullptr);
