diff --git a/label_manager/CHANGELOG.rst b/label_manager/CHANGELOG.rst index 598fe66..a3f4cb9 100644 --- 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 index e007e37..bc6d9dd 100644 --- a/label_manager/package.xml +++ b/label_manager/package.xml @@ -2,7 +2,7 @@ label_manager - 2.0.1 + 2.1.0 Serving and persisting label information Alexander Mock Matthias Holoch diff --git a/mesh_msgs/CHANGELOG.rst b/mesh_msgs/CHANGELOG.rst index ef51f77..5265be2 100644 --- 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 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/package.xml b/mesh_msgs/package.xml index db52337..bad27f1 100644 --- a/mesh_msgs/package.xml +++ b/mesh_msgs/package.xml @@ -2,7 +2,7 @@ mesh_msgs - 2.0.1 + 2.1.0 Various Message Types for Mesh Data. Alexander Mock Matthias Holoch diff --git a/mesh_msgs_conversions/CHANGELOG.rst b/mesh_msgs_conversions/CHANGELOG.rst index f021ce5..42ebba8 100644 --- 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 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/mesh_msgs_conversions/package.xml b/mesh_msgs_conversions/package.xml index 25dbfbb..aa3a927 100644 --- a/mesh_msgs_conversions/package.xml +++ b/mesh_msgs_conversions/package.xml @@ -2,7 +2,7 @@ mesh_msgs_conversions - 2.0.1 + 2.1.0 converts point clouds and attributes into meshes and mesh attributes Alexander Mock diff --git a/mesh_msgs_transform/CHANGELOG.rst b/mesh_msgs_transform/CHANGELOG.rst index 58922b7..874e219 100644 --- 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 index 953b439..dd7747c 100644 --- a/mesh_msgs_transform/package.xml +++ b/mesh_msgs_transform/package.xml @@ -2,7 +2,7 @@ mesh_msgs_transform - 2.0.1 + 2.1.0 Methods to transform mesh_msgs Sebastian Pütz BSD-3 diff --git a/mesh_tools/CHANGELOG.rst b/mesh_tools/CHANGELOG.rst index 55eb7ad..cd4d21e 100644 --- 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 index d34b2fe..4ca4af6 100644 --- a/mesh_tools/package.xml +++ b/mesh_tools/package.xml @@ -2,7 +2,7 @@ mesh_tools - 2.0.1 + 2.1.0 The mesh_tools package Sebastian Pütz BSD-3 diff --git a/rviz_mesh_tools_plugins/CHANGELOG.rst b/rviz_mesh_tools_plugins/CHANGELOG.rst index 70eb307..6e197f5 100644 --- 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 index c053f45..564b936 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 @@ -101,6 +102,8 @@ #include #include +#include + #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 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 +450,13 @@ 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; + + /// Cache for batching cost updates + ThreadSafeQueue 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 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/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp new file mode 100644 index 0000000..888d58d --- /dev/null +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.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/package.xml b/rviz_mesh_tools_plugins/package.xml index cbec81c..b9340ef 100644 --- a/rviz_mesh_tools_plugins/package.xml +++ b/rviz_mesh_tools_plugins/package.xml @@ -5,7 +5,7 @@ RViz display types and tools for the mesh_msgs package. - 2.0.1 + 2.1.0 Alexander Mock Matthias Holoch Sebastian Pütz diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml index a35eee5..36ccf48 100644 --- a/rviz_mesh_tools_plugins/plugins_description.xml +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -33,6 +33,7 @@ Displays a mesh. + mesh_msgs/msg/MeshGeometryStamped 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( diff --git a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp index f2a6156..9260c9a 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,9 +104,11 @@ 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_timeSinceLastUpdateApply(0.0) +, 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 +185,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.", @@ -271,6 +284,7 @@ void MeshDisplay::onInitialize() m_meshTopic->initialize(context_->getRosNodeAbstraction()); m_vertexColorsTopic->initialize(context_->getRosNodeAbstraction()); m_vertexCostsTopic->initialize(context_->getRosNodeAbstraction()); + m_vertexCostUpdateTopic->initialize(context_->getRosNodeAbstraction()); m_meshTopicQos->initialize( [this](rclcpp::QoS profile) { @@ -290,6 +304,12 @@ void MeshDisplay::onInitialize() updateVertexCostsSubscription(); }); + m_vertexCostUpdateTopicQos->initialize( + [this](rclcpp::QoS profile) { + m_vertexCostUpdateQos = profile; + updateVertexCostsUpdateSubscription(); + }); + // Initialize service clients //m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); //m_materialsClient = node->create_client(m_materialServiceName->getStdString()); @@ -300,9 +320,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(); } @@ -393,6 +422,21 @@ void MeshDisplay::fixedFrameChanged() this->transformMesh(); } +void MeshDisplay::setTopic(const QString& topic, const QString& datatype) +{ + (void) datatype; + RCLCPP_DEBUG( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + // The char array returned by QString.data() may not be '\0' terminated! -> topic.toStdString().c_str() + "MeshDisplay::setTopic() - called with topic='%s'", topic.toStdString().c_str() + ); + if (m_meshTopic) + { + // This also triggers the updateMeshGeometrySubscription() slot + m_meshTopic->setString(topic); + } +} + void MeshDisplay::reset() { Display::reset(); @@ -438,6 +482,7 @@ void MeshDisplay::updateAllSubscriptions() updateMeshGeometrySubscription(); updateVertexColorsSubscription(); updateVertexCostsSubscription(); + updateVertexCostsUpdateSubscription(); // TODO // initialServiceCall(); @@ -448,6 +493,7 @@ void MeshDisplay::unsubscribe() m_meshSubscriber.unsubscribe(); m_vertexColorsSubscriber.unsubscribe(); m_vertexCostsSubscriber.unsubscribe(); + m_vertexCostUpdateSubscriber.unsubscribe(); m_tfMeshFilter.reset(); m_colorsMsgCache.reset(); @@ -775,14 +821,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 +1066,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 +1082,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 +1127,145 @@ 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; + } + + 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()); + + // Space to combine updates per layer + std::map> combined; + + for (const auto& update: updates) + { + // 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]); + } + } + + for (const auto& [type, update]: combined) + { + // Vertex cost have to be applied to the cached layer + auto result = m_costCache.find(type); + // Unknown layer + if (result == m_costCache.end()) + { + //TODO: We could create an empty layer + return; + } + + // 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_costUseCustomLimits->getBool()) + { + if (m_costCache.count(m_selectVertexCostMap->getStdString()) != 0) + { + 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() + ); + } + } + } + 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) + { + 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(); + } + } + +} + 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..eecbf38 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -1066,6 +1066,96 @@ 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; + } + + /* 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(); + 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); + 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 @@ -1102,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);