Sparse Vertex Cost Message for Cost Updates and Rviz Support for Dynamic Updates

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()

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

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

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)
 {

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/ThreadSaveQueue.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"
@@ -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;
 
@@ -308,6 +314,11 @@ private Q_SLOTS:
    */
   void updateVertexCostsSubscription();
 
+  /**
+   * @brief Updates the subscribed vertex costs update topic.
+   */
+  void updateVertexCostsUpdateSubscription();
+
   /**
    * @brief Updates the vertex color service.
    */
@@ -363,6 +374,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 +435,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 +445,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 +507,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;
 

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.
    *

rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.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

rviz_mesh_tools_plugins/src/MapDisplay.cpp

@@ -448,8 +448,18 @@ bool MapDisplay::loadData()
       const std::string mesh_part = "mesh";
 
       hdf5_mesh_io->open(mapFile);
+      hdf5_mesh_io->setMeshName(mesh_part);
       auto mesh_buffer = hdf5_mesh_io->MeshIO::load(mesh_part);
 
+      // MeshIO Loads colors as bool channel so we need to load it again.
+      // This is because HDF5 stores bools and uchars as the same data type and
+      // bool loading is tried before uchar
+      auto color_opt = hdf5_mesh_io->loadChannel<unsigned char>(mesh_part + "/vertex_attributes", "vertex_colors");
+      if (color_opt)
+      {
+        mesh_buffer->insert_or_assign("vertex_colors", color_opt.value());
+      }
+
       if (nullptr == mesh_buffer)
       {
         RCLCPP_ERROR(