Merge pull request #62 from naturerobots/feature/dynamic-cost-updates

amock · web-flow · commit ed3d28964b05 · 2025-08-11T14:54:19.000+02:00
Feature: Message Definition and Visualization Support for Dynamic Cost Updates
diff --git a/label_manager/CHANGELOG.rst b/label_manager/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package label_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/label_manager/package.xml b/label_manager/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>label_manager</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Serving and persisting label information</description>
   <maintainer email="amock@uos.de">Alexander Mock</maintainer>
   <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
diff --git a/mesh_msgs/CHANGELOG.rst b/mesh_msgs/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package mesh_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/mesh_msgs/CMakeLists.txt b/mesh_msgs/CMakeLists.txt
@@ -29,6 +29,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/MeshVertexColorsStamped.msg"
   "msg/MeshVertexCosts.msg"
   "msg/MeshVertexCostsStamped.msg"
+  "msg/MeshVertexCostsSparse.msg"
+  "msg/MeshVertexCostsSparseStamped.msg"
   "msg/MeshTexture.msg"
   "msg/MeshTriangleIndices.msg"
   "msg/VectorField.msg"
@@ -49,4 +51,4 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 )
 
 ament_export_dependencies(rosidl_default_runtime)
-ament_package()
+ament_package()
diff --git a/mesh_msgs/msg/MeshVertexCostsSparse.msg b/mesh_msgs/msg/MeshVertexCostsSparse.msg
@@ -0,0 +1,11 @@
+# MeshVertexCostsSparse
+# Holds costs for a subset of the mesh vertices
+
+# The vertex indices
+uint32[] vertices
+
+# The cost per vertex
+float32[] costs
+
+# The default value for all other vertices
+float32 default_value
diff --git a/mesh_msgs/msg/MeshVertexCostsSparseStamped.msg b/mesh_msgs/msg/MeshVertexCostsSparseStamped.msg
@@ -0,0 +1,5 @@
+# Mesh Attribute Message
+std_msgs/Header header
+string uuid
+string type
+mesh_msgs/MeshVertexCostsSparse mesh_vertex_costs
diff --git a/mesh_msgs/package.xml b/mesh_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>mesh_msgs</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Various Message Types for Mesh Data.</description>
   <maintainer email="amock@uos.de">Alexander Mock</maintainer>
   <maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
diff --git a/mesh_msgs_conversions/CHANGELOG.rst b/mesh_msgs_conversions/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package mesh_msgs_conversions
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h
@@ -62,6 +62,7 @@
 #include "mesh_msgs/msg/mesh_vertex_colors.hpp"
 #include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp"
 #include "mesh_msgs/msg/mesh_vertex_costs_stamped.hpp"
+#include "mesh_msgs/msg/mesh_vertex_costs_sparse_stamped.hpp"
 #include "mesh_msgs/msg/mesh_vertex_tex_coords.hpp"
 #include "mesh_msgs/msg/mesh_material.hpp"
 #include "mesh_msgs/msg/mesh_texture.hpp"
@@ -224,6 +225,31 @@ inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts(
   return costs_msg;
 }
 
+inline const mesh_msgs::msg::MeshVertexCostsSparse toVertexCostsSparse(
+    const lvr2::VertexMap<float>& costs,
+    const float default_value
+)
+{
+    mesh_msgs::msg::MeshVertexCostsSparse costs_msg;
+    costs_msg.vertices.resize(costs.numValues());
+    costs_msg.costs.resize(costs.numValues());
+    costs_msg.default_value = default_value;
+
+    std::transform(costs.begin(), costs.end(), costs_msg.vertices.begin(),
+        [](const lvr2::VertexHandle& v)
+        {
+            return v.idx();
+        }
+    );
+    std::transform(costs_msg.vertices.begin(), costs_msg.vertices.end(), costs_msg.costs.begin(),
+        [&costs](const uint32_t idx)
+        {
+            return costs[lvr2::VertexHandle(idx)];
+        }
+    );
+    return costs_msg;
+}
+
 inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped(
     const lvr2::VertexMap<float>& costs,
     const size_t num_values,
@@ -243,6 +269,24 @@ inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped(
   return mesh_msg;
 }
 
+inline const mesh_msgs::msg::MeshVertexCostsSparseStamped toVertexCostsSparseStamped(
+    const lvr2::VertexMap<float>& costs,
+    const float default_value,
+    const std::string& name,
+    const std::string& frame_id,
+    const std::string& uuid,
+    const rclcpp::Time& stamp = rclcpp::Time()
+)
+{
+  mesh_msgs::msg::MeshVertexCostsSparseStamped msg;
+  msg.mesh_vertex_costs = toVertexCostsSparse(costs, default_value);
+  msg.uuid = uuid;
+  msg.type = name;
+  msg.header.frame_id = frame_id;
+  msg.header.stamp = stamp;
+  return msg;
+}
+
 inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts(
     const lvr2::DenseVertexMap<float>& costs)
 {
diff --git a/mesh_msgs_conversions/package.xml b/mesh_msgs_conversions/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>mesh_msgs_conversions</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>converts point clouds and attributes into meshes and mesh attributes</description>
 
   <maintainer email="amock@uos.de">Alexander Mock</maintainer>
diff --git a/mesh_msgs_transform/CHANGELOG.rst b/mesh_msgs_transform/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package mesh_msgs_transform
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/mesh_msgs_transform/package.xml b/mesh_msgs_transform/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>mesh_msgs_transform</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Methods to transform mesh_msgs</description>
   <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
   <license>BSD-3</license>
diff --git a/mesh_tools/CHANGELOG.rst b/mesh_tools/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package ros_mesh_tools
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/mesh_tools/package.xml b/mesh_tools/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>mesh_tools</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>The mesh_tools package</description>
   <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
   <license>BSD-3</license>
diff --git a/rviz_mesh_tools_plugins/CHANGELOG.rst b/rviz_mesh_tools_plugins/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package rviz_mesh_tools_plugins
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2025-08-11)
+------------------
+* Add MeshVertexCostsSparse and MeshVertexCostsSparseStamped messages
+* Add support for visualizing cost map updates in RViz using the MeshDisplay
+
 2.0.1 (2025-08-11)
 ------------------
 * Changes since the first ROS 2 port to enable the same functionality as the ROS 1 version
diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp
@@ -85,6 +85,7 @@
 #include <message_filters/cache.h>
 
 #include <rviz_mesh_tools_plugins/RVizMessageFilter.hpp>
+#include <rviz_mesh_tools_plugins/ThreadSafeQueue.hpp>
 
 #include <rviz_rendering/mesh_loader.hpp>
 
@@ -101,6 +102,8 @@
 #include <mesh_msgs/srv/get_texture.hpp>
 #include <mesh_msgs/srv/get_uui_ds.hpp>
 
+#include <mesh_msgs/msg/mesh_vertex_costs_sparse_stamped.hpp>
+
 #endif // Q_MOC_RUN
 
 #include "rclcpp/rclcpp.hpp"
@@ -140,7 +143,7 @@ class MeshDisplay : public rviz_common::Display
   Q_OBJECT
 
 public:
-  /**#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
+  /**
    * @brief Constructor
    */
   MeshDisplay();
@@ -154,6 +157,9 @@ class MeshDisplay : public rviz_common::Display
 
   /**
    * @brief Periodically called from rviz
+   *
+   * @param wall_dt Time since last update in NANOSECONDS (rviz doc says seconds)
+   * @param ros_dt Same as wall_dt but in ROS Time also NANOSECONDS
    */
   void update(float wall_dt, float ros_dt) override;
 
@@ -171,6 +177,11 @@ class MeshDisplay : public rviz_common::Display
 
   void fixedFrameChanged() override;
 
+  /**
+   * @brief Used by RViz's "New display by topic" window
+   */
+  void setTopic(const QString& topic, const QString& datatype) override;
+
   /**
    * @brief Update all subscriptions. Individual subscription update function will check whether they are active. (e.g. vertex colors can also be inactive when the UI element is set to a fixed color).
    */
@@ -308,6 +319,11 @@ private Q_SLOTS:
    */
   void updateVertexCostsSubscription();
 
+  /**
+   * @brief Updates the subscribed vertex costs update topic.
+   */
+  void updateVertexCostsUpdateSubscription();
+
   /**
    * @brief Updates the vertex color service.
    */
@@ -363,6 +379,17 @@ private Q_SLOTS:
    */
   void vertexCostsCallback(const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costsStamped);
 
+  /**
+   * @brief Handler for incoming vertex cost update messages. Validate data and update the cost layer
+   * @param costsStamped The vertex costs
+   */
+  void vertexCostsUpdateCallback(const mesh_msgs::msg::MeshVertexCostsSparseStamped::ConstSharedPtr& update);
+
+  /**
+   * @brief Apply all cached vertex cost updates
+   */
+  void applyCachedCostUpdates();
+
   /**
    * @brief Requests vertex colors from the specified service
    * @param uuid Mesh UUID
@@ -413,6 +440,8 @@ private Q_SLOTS:
   message_filters::Subscriber<mesh_msgs::msg::MeshVertexColorsStamped> m_vertexColorsSubscriber;
   /// Subscriber for vertex costs
   message_filters::Subscriber<mesh_msgs::msg::MeshVertexCostsStamped> m_vertexCostsSubscriber;
+  /// Subscriber for vertex cost updates
+  message_filters::Subscriber<mesh_msgs::msg::MeshVertexCostsSparseStamped> m_vertexCostUpdateSubscriber;
 
   /// TF2 message filter for incoming mesh data. Ensures we only process meshes for which a suitable TF is available
   tf2_ros::RVizMessageFilterPtr<mesh_msgs::msg::MeshGeometryStamped> m_tfMeshFilter;
@@ -421,6 +450,13 @@ private Q_SLOTS:
   std::shared_ptr<message_filters::Cache<mesh_msgs::msg::MeshVertexColorsStamped>> m_colorsMsgCache;
   /// Cache for vertex costs, useful for when cost information arrives before the mesh geometry
   std::shared_ptr<message_filters::Cache<mesh_msgs::msg::MeshVertexCostsStamped>> m_costsMsgCache;
+  /// Cache for vertex costs updates
+  std::shared_ptr<message_filters::Cache<mesh_msgs::msg::MeshVertexCostsSparseStamped>> m_costsUpdateMsgCache;
+
+  /// Cache for batching cost updates
+  ThreadSafeQueue<mesh_msgs::msg::MeshVertexCostsSparseStamped::ConstSharedPtr> m_costsUpdateCache;
+  float m_timeSinceLastUpdateApply;
+
 
   /// Counter for the received messages
   uint32_t m_messagesReceived;
@@ -476,6 +512,11 @@ private Q_SLOTS:
   rviz_common::properties::QosProfileProperty* m_vertexCostsTopicQos;
   rclcpp::QoS m_vertexCostsQos;
 
+  /// Properties to handle topic vor vertex cost updates
+  rviz_common::properties::RosTopicProperty* m_vertexCostUpdateTopic;
+  rviz_common::properties::QosProfileProperty* m_vertexCostUpdateTopicQos;
+  rclcpp::QoS m_vertexCostUpdateQos;
+
   /// Property to select different types of vertex cost maps to be shown
   rviz_common::properties::EnumProperty* m_selectVertexCostMap;
 
diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp
@@ -166,6 +166,23 @@ class MeshVisual
    */
   bool setVertexCosts(const std::vector<float>& vertexCosts, int costColorType, float minCost, float maxCost);
 
+  /**
+   * @brief Update a subset of the vertex costs
+   *
+   * @param vertices Vector containing the vertex indices to update
+   * @param costs Vector containing the vertex cost information
+   * @param costColorType colorization method (0 = rainbow; 1 = red-green)
+   * @param minCost minimum value for colorization
+   * @param maxCost maximum value for colorization
+   */
+  bool updateVertexCosts(
+    const std::vector<uint32_t>& vertices,
+    const std::vector<float>& costs,
+    int costColorType,
+    float minCost,
+    float maxCost
+  );
+
   /**
    * @brief Extracts data from the ros-messages and creates a textured mesh.
    *
diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <queue>
+#include <mutex>
+
+namespace rviz_mesh_tools_plugins
+{
+
+
+template <typename T>
+class ThreadSafeQueue: protected std::queue<T>
+{
+public:
+  T pop()
+  {
+    std::lock_guard lock(mutex_);
+    T elem = this->std::queue<T>::front();
+    this->std::queue<T>::pop();
+    return elem;
+  }
+
+  std::vector<T> popAll()
+  {
+    std::lock_guard lock(mutex_);
+    std::vector<T> data;
+    data.reserve(this->size());
+
+    while(!this->empty())
+    {
+      data.push_back(this->std::queue<T>::front());
+      this->std::queue<T>::pop();
+    }
+    return data;
+  }
+
+  void push(const T& elem)
+  {
+    std::lock_guard lock(mutex_);
+    this->std::queue<T>::push(elem);
+  }
+
+private:
+  std::mutex mutex_;
+};
+
+} // namespace rviz_mesh_tools_plugins
diff --git a/rviz_mesh_tools_plugins/package.xml b/rviz_mesh_tools_plugins/package.xml
diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml
diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp
diff --git a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp
diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp
