Add MeshVertexCostsSparse message to efficiently update only changed

vertex costs

Author: JustusBraun

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

@@ -101,6 +101,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"
@@ -308,6 +310,11 @@ private Q_SLOTS:
    */
   void updateVertexCostsSubscription();
 
+  /**
+   * @brief Updates the subscribed vertex costs update topic.
+   */
+  void updateVertexCostsUpdateSubscription();
+
   /**
    * @brief Updates the vertex color service.
    */
@@ -363,6 +370,12 @@ 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 Requests vertex colors from the specified service
    * @param uuid Mesh UUID
@@ -413,6 +426,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 +436,8 @@ 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;
 
   /// Counter for the received messages
   uint32_t m_messagesReceived;
@@ -476,6 +493,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/src/MeshDisplay.cpp

@@ -102,9 +102,10 @@ namespace rviz_mesh_tools_plugins
 MeshDisplay::MeshDisplay() 
 : rviz_common::Display()
 , m_ignoreMsgs(false)
-, m_meshQos(rclcpp::SystemDefaultsQoS())
-, m_vertexColorsQos(rclcpp::SystemDefaultsQoS())
-, m_vertexCostsQos(rclcpp::SystemDefaultsQoS())
+, m_meshQos(rclcpp::QoS(1).transient_local())
+, m_vertexColorsQos(rclcpp::QoS(5))
+, m_vertexCostsQos(rclcpp::QoS(5))
+, m_vertexCostUpdateQos(rclcpp::QoS(10))
 {
   // mesh topic
   m_meshTopic = new rviz_common::properties::RosTopicProperty(
@@ -181,12 +182,21 @@ MeshDisplay::MeshDisplay()
 
 
       m_vertexCostsTopic = new rviz_common::properties::RosTopicProperty(
-          "Vertex Costs Topic", "",
+          "Costs Topic", "",
           QString::fromStdString(rosidl_generator_traits::name<mesh_msgs::msg::MeshVertexCostsStamped>()),
           "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsSubscription()), this);
 
       m_vertexCostsTopicQos = new rviz_common::properties::QosProfileProperty(m_vertexCostsTopic, m_vertexCostsQos);
 
+      m_vertexCostUpdateTopic = new rviz_common::properties::RosTopicProperty(
+        "Update Topic", "",
+        QString::fromStdString(
+          rosidl_generator_traits::name<mesh_msgs::msg::MeshVertexCostsSparseStamped>()
+        ),
+        "Vertex cost update topic to subscribe to.", m_displayType, SLOT(updateVertexCostsUpdateSubscription()), this
+      );
+      m_vertexCostUpdateTopicQos = new rviz_common::properties::QosProfileProperty(m_vertexCostUpdateTopic, m_vertexCostUpdateQos);
+
       m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --",
                                                      "Select the type of vertex cost map to be displayed. New types "
                                                      "will appear here when a new message arrives.",
@@ -775,14 +785,50 @@ void MeshDisplay::updateVertexCostsSubscription()
       auto node = context_->getRosNodeAbstraction().lock()->get_raw_node();
       m_vertexCostsSubscriber.subscribe(node, m_vertexCostsTopic->getTopicStd(), m_vertexCostsQos.get_rmw_qos_profile());
 
-      m_costsMsgCache = std::make_shared<message_filters::Cache<mesh_msgs::msg::MeshVertexCostsStamped>>(m_vertexCostsSubscriber, 1);
+      // TODO: What is a good cache size? Could it be configurable by the user? Also it is not possible to clear the message cache after we processed the messages?
+      m_costsMsgCache = std::make_shared<message_filters::Cache<mesh_msgs::msg::MeshVertexCostsStamped>>(m_vertexCostsSubscriber, 10);
       m_costsMsgCache->registerCallback(std::bind(&MeshDisplay::vertexCostsCallback, this, _1));
+
+      // Update update topic
+      const std::string update_topic = m_vertexCostsTopic->getTopicStd() + "/updates";
+      m_vertexCostUpdateTopic->setStdString(update_topic);
     } else {
       setStatus(rviz_common::properties::StatusProperty::Warn, "Topic", QString("Error subscribing: Empty cost topic name"));
     }
   }
 }
 
+void MeshDisplay::updateVertexCostsUpdateSubscription()
+{
+  if (m_vertexCostUpdateTopic->getHidden())
+  {
+    return;
+  }
+
+  if (m_vertexCostUpdateTopic->getTopic().isEmpty())
+  {
+    return;
+  }
+
+  // Update the subscription
+  auto node_abstraction = context_->getRosNodeAbstraction().lock();
+  if (nullptr == node_abstraction)
+  {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rviz_mesh_tools_plugins"),
+      "[MeshDisplay::updateVertexCostsUpdateSubscription] Could not get the ROS Node abstraction instance from RViz context!"
+    );
+    return;
+  }
+
+  // Update the subscription
+  rclcpp::Node::SharedPtr node = node_abstraction->get_raw_node();
+  m_vertexCostUpdateSubscriber.subscribe(node, m_vertexCostUpdateTopic->getTopicStd(), m_vertexCostUpdateQos.get_rmw_qos_profile());
+  // Update the cache
+  m_costsUpdateMsgCache = std::make_shared<message_filters::Cache<mesh_msgs::msg::MeshVertexCostsSparseStamped>>(m_vertexCostUpdateSubscriber, 10);
+  m_costsUpdateMsgCache->registerCallback(std::bind(&MeshDisplay::vertexCostsUpdateCallback, this, _1));
+}
+
 void MeshDisplay::updateMaterialAndTextureServices()
 {
   if (m_ignoreMsgs)
@@ -984,6 +1030,7 @@ void MeshDisplay::meshGeometryCallback(
   m_messagesReceived++;
   setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received");
   processMessage(*meshMsg);
+  const rclcpp::Time now = context_->getClock()->now();
 
   // check for cached color and cost msgs that might have arrived earlier:
   if (m_colorsMsgCache) {
@@ -999,8 +1046,8 @@ void MeshDisplay::meshGeometryCallback(
     }
   }
   if (m_costsMsgCache) {
-    const auto costMsgs = m_costsMsgCache->getInterval(meshMsg->header.stamp, meshMsg->header.stamp); // get msgs where header.stamp match exactly (this should usually be only one msg)
-    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Got " << costMsgs.size() << " cost msgs from cache with header.stamp=" << rclcpp::Time(meshMsg->header.stamp).seconds());
+    const auto costMsgs = m_costsMsgCache->getInterval(meshMsg->header.stamp, now);
+    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Got " << costMsgs.size() << " cost msgs from cache");
     for (const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costMsg : costMsgs) {
       if (costMsg->uuid == meshMsg->uuid) {
         RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found cached cost msg with matching UUID, applying its information now");
@@ -1044,6 +1091,92 @@ void MeshDisplay::vertexCostsCallback(
   updateVertexCosts();
 }
 
+void MeshDisplay::vertexCostsUpdateCallback(
+  const mesh_msgs::msg::MeshVertexCostsSparseStamped::ConstSharedPtr& update
+)
+{
+  if (update->uuid.compare(m_lastUuid) != 0)
+  {
+    RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received vertex costs update, but its UUID does not match the latest mesh geometry UUID. Not updating costs.");
+    return;
+  }
+
+  if (update->mesh_vertex_costs.vertices.empty() || update->mesh_vertex_costs.costs.empty())
+  {
+    RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received an empty vertex costs update.");
+    return;
+  }
+
+  // Vertex cost have to be applied to the cached layer
+  auto result = m_costCache.find(update->type);
+  // Unknown layer
+  if (result == m_costCache.end())
+  {
+    //TODO: We could create an empty layer
+    return;
+  }
+
+  // Update data in the cache
+  const auto& data = update->mesh_vertex_costs;
+  for (size_t i = 0; i < data.vertices.size(); i++)
+  {
+    result->second[data.vertices[i]] = data.costs[i];
+  }
+
+  // Update the mesh if we updated the current visual
+  if (update->type == m_selectVertexCostMap->getStdString())
+  {
+    if (m_costUseCustomLimits->getBool())
+    {
+      if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0)
+      {
+        std::shared_ptr<MeshVisual> visual = getLatestVisual();
+        if (visual)
+        {
+          visual->updateVertexCosts(
+            data.vertices,
+            data.costs,
+            m_costColorType->getOptionInt(),
+            m_costLowerLimit->getFloat(),
+            m_costUpperLimit->getFloat()
+          );
+        }
+      }
+    }
+    else
+    {
+      if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0)
+      {
+        // The correct way is the get the limits from the whole layer
+        // TODO: Cache this with the layer itself
+        float maxCost = std::numeric_limits<float>::min();
+        float minCost = std::numeric_limits<float>::max();
+        for (float cost : result->second)
+        {
+          if (std::isfinite(cost) && cost > maxCost)
+            maxCost = cost;
+          if (std::isfinite(cost) && cost < minCost)
+            minCost = cost;
+        }
+
+
+        std::shared_ptr<MeshVisual> visual = getLatestVisual();
+        if (visual)
+        {
+          visual->updateVertexCosts(
+            data.vertices,
+            data.costs,
+            m_costColorType->getOptionInt(),
+            minCost,
+            maxCost
+          );
+        }
+      }
+    }
+    updateMeshMaterial();
+  }
+}
+
 void MeshDisplay::requestVertexColors(std::string uuid)
 {
   if (m_ignoreMsgs)