From 696d75f0bcfecbfdcae5e4e0db2d857bb6c60543 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Wed, 5 Mar 2025 16:09:05 +0100 Subject: [PATCH 1/4] Add MeshVertexCostsSparse message to efficiently update only changed vertex costs --- mesh_msgs/CMakeLists.txt | 4 +- mesh_msgs/msg/MeshVertexCostsSparse.msg | 11 ++ .../msg/MeshVertexCostsSparseStamped.msg | 5 + .../mesh_msgs_conversions/conversions.h | 44 ++++++ .../rviz_mesh_tools_plugins/MeshDisplay.hpp | 22 +++ .../rviz_mesh_tools_plugins/MeshVisual.hpp | 17 ++ rviz_mesh_tools_plugins/src/MeshDisplay.cpp | 147 +++++++++++++++++- rviz_mesh_tools_plugins/src/MeshVisual.cpp | 62 ++++++++ 8 files changed, 304 insertions(+), 8 deletions(-) create mode 100644 mesh_msgs/msg/MeshVertexCostsSparse.msg create mode 100644 mesh_msgs/msg/MeshVertexCostsSparseStamped.msg diff --git a/mesh_msgs/CMakeLists.txt b/mesh_msgs/CMakeLists.txt index edad5a4..dfa9108 100644 --- 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() \ No newline at end of file +ament_package() diff --git a/mesh_msgs/msg/MeshVertexCostsSparse.msg b/mesh_msgs/msg/MeshVertexCostsSparse.msg new file mode 100644 index 0000000..80e12d1 --- /dev/null +++ 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 new file mode 100644 index 0000000..3c70000 --- /dev/null +++ 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_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h index d677c14..a5fb491 100644 --- 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& 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& 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& 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& costs) { 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 index c053f45..8999f86 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp @@ -101,6 +101,8 @@ #include #include +#include + #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 m_vertexColorsSubscriber; /// Subscriber for vertex costs message_filters::Subscriber m_vertexCostsSubscriber; + /// Subscriber for vertex cost updates + message_filters::Subscriber m_vertexCostUpdateSubscriber; /// TF2 message filter for incoming mesh data. Ensures we only process meshes for which a suitable TF is available tf2_ros::RVizMessageFilterPtr m_tfMeshFilter; @@ -421,6 +436,8 @@ private Q_SLOTS: std::shared_ptr> m_colorsMsgCache; /// Cache for vertex costs, useful for when cost information arrives before the mesh geometry std::shared_ptr> m_costsMsgCache; + /// Cache for vertex costs updates + std::shared_ptr> 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; 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 index f507398..4d8063e 100644 --- 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& 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& vertices, + const std::vector& 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/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp index f2a6156..dc17221 100644 --- a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp +++ b/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()), "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() + ), + "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>(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>(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>(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 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::min(); + float minCost = std::numeric_limits::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 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) diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index 0c76ed2..c8f1275 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -1066,6 +1066,68 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC return true; } +bool MeshVisual::updateVertexCosts( + const std::vector& vertices, + const std::vector& costs, + int costColorType, + float minCost, + float maxCost +) +{ + + float range = maxCost - minCost; + if (range <= 0) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Illegal vertex cost limits!"); + return false; + } + + if (!m_vertexCostMaterial) + { + // We only update existing materials, the caller has to make sure that the layer already exists + return false; + } + + // Get the needed information about the raw vertex buffer + Ogre::RenderOperation* render_op = m_vertexCostsMesh->getSections().front()->getRenderOperation(); + // Information about the Vertex Colour attribute + const Ogre::VertexDeclaration* v_decl = render_op->vertexData->vertexDeclaration; + const Ogre::VertexElement* color_sem = v_decl->findElementBySemantic(Ogre::VES_COLOUR); + + // Get and lock the hardware vertex buffer of the mesh + Ogre::HardwareVertexBufferSharedPtr vbuf = render_op->vertexData->vertexBufferBinding->getBuffer(0); + Ogre::HardwareBufferLockGuard vbuf_lock(vbuf, Ogre::HardwareBuffer::HBL_WRITE_ONLY); + std::byte* raw = reinterpret_cast(vbuf_lock.pData); + + // write vertex colors + const auto& mesh = m_geometry; + for (uint32_t i = 0; i < vertices.size(); i++) + { + // Vertex handle + const uint32_t v = vertices[i]; + + if (v >= mesh.vertices.size()) + { + continue; + } + + // Point to the beginning of the vertex + std::byte* vertex = raw + v * vbuf->getVertexSize(); + // Pointer to the color + Ogre::ABGR* vcolor = nullptr; + color_sem->baseVertexPointerToElement(vertex, &vcolor); + + // write vertex colors that are calculated from the cost values + float normalizedCost = (costs[i] - minCost) / range; + normalizedCost = std::max(0.0f, normalizedCost); + normalizedCost = std::min(1.0f, normalizedCost); + + *vcolor = calculateColorFromCost(normalizedCost, costColorType).getAsABGR(); + } + + return true; +} + bool MeshVisual::setMaterials(const vector& materials, const vector& texCoords) { // check if there is a material index for each cluster From 96cf4de4cbd18e0fac386ba8090860bee952dd22 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Fri, 9 May 2025 16:57:20 +0200 Subject: [PATCH 2/4] Process Cost Updates in batches to prevent locking the VertexBuffer over and over again --- .../rviz_mesh_tools_plugins/MeshDisplay.hpp | 14 ++ .../ThreadSaveQueue.hpp | 46 +++++ rviz_mesh_tools_plugins/src/MeshDisplay.cpp | 171 ++++++++++++------ 3 files changed, 178 insertions(+), 53 deletions(-) create mode 100644 rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp 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 index 8999f86..b4e6c92 100644 --- 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 #include +#include #include @@ -156,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; @@ -376,6 +380,11 @@ private Q_SLOTS: */ 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 @@ -439,6 +448,11 @@ private Q_SLOTS: /// Cache for vertex costs updates std::shared_ptr> m_costsUpdateMsgCache; + /// Cache for batching cost updates + ThreadSafeQueue m_costsUpdateCache; + float m_timeSinceLastUpdateApply; + + /// Counter for the received messages uint32_t m_messagesReceived; /// Uuid of the last received message diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp new file mode 100644 index 0000000..888d58d --- /dev/null +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include + +namespace rviz_mesh_tools_plugins +{ + + +template +class ThreadSafeQueue: protected std::queue +{ +public: + T pop() + { + std::lock_guard lock(mutex_); + T elem = this->std::queue::front(); + this->std::queue::pop(); + return elem; + } + + std::vector popAll() + { + std::lock_guard lock(mutex_); + std::vector data; + data.reserve(this->size()); + + while(!this->empty()) + { + data.push_back(this->std::queue::front()); + this->std::queue::pop(); + } + return data; + } + + void push(const T& elem) + { + std::lock_guard lock(mutex_); + this->std::queue::push(elem); + } + +private: + std::mutex mutex_; +}; + +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp index dc17221..1ca30dd 100644 --- a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp @@ -82,6 +82,8 @@ #include "rclcpp/executor.hpp" #include "rmw/validate_full_topic_name.h" +#include + using namespace std::chrono_literals; using std::placeholders::_1; @@ -102,6 +104,7 @@ namespace rviz_mesh_tools_plugins MeshDisplay::MeshDisplay() : rviz_common::Display() , m_ignoreMsgs(false) +, m_timeSinceLastUpdateApply(0.0) , m_meshQos(rclcpp::QoS(1).transient_local()) , m_vertexColorsQos(rclcpp::QoS(5)) , m_vertexCostsQos(rclcpp::QoS(5)) @@ -310,9 +313,18 @@ void MeshDisplay::onInitialize() void MeshDisplay::update(float wall_dt, float ros_dt) { - (void) wall_dt; (void) ros_dt; + if (m_timeSinceLastUpdateApply >= 1.0e9) + { + this->applyCachedCostUpdates(); + m_timeSinceLastUpdateApply = 0.0; + } + else + { + m_timeSinceLastUpdateApply += wall_dt; + } + this->transformMesh(); } @@ -1107,74 +1119,127 @@ void MeshDisplay::vertexCostsUpdateCallback( 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; - } + this->m_costsUpdateCache.push(update); +} + +void MeshDisplay::applyCachedCostUpdates() +{ + + const auto updates = m_costsUpdateCache.popAll(); + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Processing %lu updates", updates.size()); - // Update data in the cache - const auto& data = update->mesh_vertex_costs; - for (size_t i = 0; i < data.vertices.size(); i++) + // Space to combine updates per layer + std::map> combined; + + for (const auto& update: updates) { - result->second[data.vertices[i]] = data.costs[i]; + // Vertex cost have to be applied to the cached layer + auto result = combined.find(update->type); + // Unknown layer + if (result == combined.end()) + { + result = combined.insert({update->type, lvr2::SparseVertexMap()}).first; + } + + // Update data in the cache + const auto& data = update->mesh_vertex_costs; + for (size_t i = 0; i < data.vertices.size(); i++) + { + result->second.insert(lvr2::VertexHandle(data.vertices[i]), data.costs[i]); + } } - // Update the mesh if we updated the current visual - if (update->type == m_selectVertexCostMap->getStdString()) + for (const auto& [type, update]: combined) { - if (m_costUseCustomLimits->getBool()) + // Vertex cost have to be applied to the cached layer + auto result = m_costCache.find(type); + // Unknown layer + if (result == m_costCache.end()) { - if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0) - { - std::shared_ptr visual = getLatestVisual(); - if (visual) - { - visual->updateVertexCosts( - data.vertices, - data.costs, - m_costColorType->getOptionInt(), - m_costLowerLimit->getFloat(), - m_costUpperLimit->getFloat() - ); - } - } + //TODO: We could create an empty layer + return; } - else + + // Update data in the cache + for (lvr2::VertexHandle v: update) + { + result->second[v.idx()] = update[v]; + } + + // Update the mesh if we updated the current visual + if (type == m_selectVertexCostMap->getStdString()) { - if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0) + if (m_costUseCustomLimits->getBool()) { - // The correct way is the get the limits from the whole layer - // TODO: Cache this with the layer itself - float maxCost = std::numeric_limits::min(); - float minCost = std::numeric_limits::max(); - for (float cost : result->second) + if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0) { - if (std::isfinite(cost) && cost > maxCost) - maxCost = cost; - if (std::isfinite(cost) && cost < minCost) - minCost = cost; + std::shared_ptr visual = getLatestVisual(); + if (visual) + { + std::vector vertices; + std::vector costs; + vertices.reserve(update.numValues()); + costs.reserve(update.numValues()); + for (lvr2::VertexHandle v: update) + { + vertices.push_back(v.idx()); + costs.push_back(update[v]); + } + + visual->updateVertexCosts( + vertices, + costs, + m_costColorType->getOptionInt(), + m_costLowerLimit->getFloat(), + m_costUpperLimit->getFloat() + ); + } } - - - std::shared_ptr visual = getLatestVisual(); - if (visual) + } + else + { + if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0) { - visual->updateVertexCosts( - data.vertices, - data.costs, - m_costColorType->getOptionInt(), - minCost, - maxCost - ); + // The correct way is the get the limits from the whole layer + // TODO: Cache this with the layer itself + float maxCost = std::numeric_limits::min(); + float minCost = std::numeric_limits::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 visual = getLatestVisual(); + if (visual) + { + std::vector vertices; + std::vector costs; + vertices.reserve(update.numValues()); + costs.reserve(update.numValues()); + for (lvr2::VertexHandle v: update) + { + vertices.push_back(v.idx()); + costs.push_back(update[v]); + } + visual->updateVertexCosts( + vertices, + costs, + m_costColorType->getOptionInt(), + minCost, + maxCost + ); + } } } + updateMeshMaterial(); } - updateMeshMaterial(); } + } void MeshDisplay::requestVertexColors(std::string uuid) From 17da0ce0f3793fd01a95f32f639cbac099645e0c Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Fri, 20 Jun 2025 17:44:51 +0200 Subject: [PATCH 3/4] Fix color loading from H5 --- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 04bf9cd..634d033 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/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(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( From bf31bc719c0760c41b094fc4ecfc36efed5d1d79 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Tue, 5 Aug 2025 13:45:41 +0200 Subject: [PATCH 4/4] Backport to ROS Humble --- rviz_mesh_tools_plugins/src/MeshVisual.cpp | 35 ++++++++++++++++++---- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index c8f1275..eecbf38 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -1088,11 +1088,39 @@ bool MeshVisual::updateVertexCosts( return false; } - // Get the needed information about the raw vertex buffer + /* ManualObject::getNumSections and ManualObject::getSection are deprecated + * in the Ogre version ROS Jazzy (and future versions) uses. Since the new + * API is not available in the Ogre version used by ROS Humble we use this + * check to keep Humble support. + * + * This can be removed when Humble support is dropped. + */ +#if OGRE_VERSION < ((1 << 16) | (12 << 8) | 7) + Ogre::RenderOperation* render_op = m_vertexCostsMesh->getSection(0)->getRenderOperation(); + const Ogre::VertexDeclaration* v_decl = render_op->vertexData->vertexDeclaration; + + // findElementBySemantic does not support VES_COLOUR yet + const Ogre::VertexElement* color_sem = nullptr; + for (const auto& elem: v_decl->getElements()) + { + // VET_COLOUR is deprecated in favour of VET_UBYTE4_NORM + if (Ogre::VET_UBYTE4_NORM == elem.getType()) + { + color_sem = &elem; + } + } +#else + // Get the needed Vertex Colour attribute information about the raw vertex buffer Ogre::RenderOperation* render_op = m_vertexCostsMesh->getSections().front()->getRenderOperation(); - // Information about the Vertex Colour attribute const Ogre::VertexDeclaration* v_decl = render_op->vertexData->vertexDeclaration; const Ogre::VertexElement* color_sem = v_decl->findElementBySemantic(Ogre::VES_COLOUR); +#endif + + if (nullptr == color_sem) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Vertex Cost Mesh has no Vertex Colour attribute!"); + return false; + } // Get and lock the hardware vertex buffer of the mesh Ogre::HardwareVertexBufferSharedPtr vbuf = render_op->vertexData->vertexBufferBinding->getBuffer(0); @@ -1164,9 +1192,6 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) { uint32_t width = texture.width; uint32_t height = texture.height; - uint32_t step = texture.channels; - - uint32_t dataSize = width * height * step; Ogre::PixelFormat pixelFormat = getOgrePixelFormatFromRosString(texture.pixelFormat);