From 16f0788d0f84580c26ac43e3766aa5d63b2d0c08 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 Mar 2026 04:16:38 +0800 Subject: [PATCH] Reintroduce pointcloud render without Float_V message (#755) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 🦟 Bug fix [This bug](https://github.com/gazebosim/gz-gui/pull/494#issuecomment-3995382967) ## Summary Re-introduce point cloud rendering without the Float_V message. Screenshot From 2026-03-17 11-49-46 I tested it with the following program: ```C++ #include #include #include #include #include #include #include int main(int argc, char** argv) { // Create a transport node. gz::transport::Node node; // Define the topic to publish on. std::string topic = "/sphere_points"; // Create a publisher for PointCloudPacked messages. auto pub = node.Advertise(topic); if (!pub) { std::cerr << "Error advertising topic [" << topic << "]" << std::endl; return -1; } // Sphere parameters double radius = 1.0; int num_theta = 20; int num_phi = 40; // Prepare the message gz::msgs::PointCloudPacked msg; // Initialize header msg.mutable_header()->mutable_stamp()->set_sec(0); msg.mutable_header()->mutable_stamp()->set_nsec(0); // Initialize PointCloudPacked gz::msgs::InitPointCloudPacked(msg, "frame_id", false, {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); int num_points = (num_theta + 1) * num_phi; msg.set_width(num_points); msg.set_height(1); msg.set_is_dense(true); msg.mutable_data()->resize(num_points * msg.point_step()); gz::msgs::PointCloudPackedIterator iterX(msg, "x"); gz::msgs::PointCloudPackedIterator iterY(msg, "y"); gz::msgs::PointCloudPackedIterator iterZ(msg, "z"); const double PI = std::acos(-1.0); for (int i = 0; i <= num_theta; ++i) { double theta = PI * i / num_theta; for (int j = 0; j < num_phi; ++j) { double phi = 2.0 * PI * j / num_phi; double x = radius * std::sin(theta) * std::cos(phi); double y = radius * std::sin(theta) * std::sin(phi); double z = radius * std::cos(theta); *iterX = static_cast(x); *iterY = static_cast(y); *iterZ = static_cast(z); ++iterX; ++iterY; ++iterZ; } } std::cout << "Publishing a sphere with " << num_points << " points on topic [" << topic << "]" << std::endl; // Publish the message in a loop while (true) { if (!pub.Publish(msg)) { std::cerr << "Failed to publish message" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } return 0; } ``` TODO: Color support: I think we should not implement the color rendering support here, rather perhaps we should have some PointCloud representation in `gz-math`. For now there is rudimentary support for colors in this PR. --------- Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Co-authored-by: Ian Chen Co-authored-by: Ian Chen (cherry picked from commit d8b6a544dc013f5c2e63e1fd5bb1b66425b9c9b3) --- src/plugins/point_cloud/PointCloud.cc | 132 +++++++++++++++++++++----- 1 file changed, 109 insertions(+), 23 deletions(-) diff --git a/src/plugins/point_cloud/PointCloud.cc b/src/plugins/point_cloud/PointCloud.cc index 6245ea691..81b7fd9b5 100644 --- a/src/plugins/point_cloud/PointCloud.cc +++ b/src/plugins/point_cloud/PointCloud.cc @@ -21,6 +21,8 @@ #include "gz/msgs/pointcloud_packed.pb.h" #include +#include +#include #include #include #include @@ -96,6 +98,11 @@ class PointCloud::Implementation /// \brief True if showing, changeable at runtime public: bool showing{true}; + + /// \brief Sometimes we may just want to have a pointcloud + /// Without a float field. This is triggered when we + /// set a scalar float topic to subscribe to. + public: bool hasFloatTopic{false}; }; ///////////////////////////////////////////////// @@ -182,6 +189,7 @@ void PointCloud::OnPointCloudTopic(const QString &_pointCloudTopic) void PointCloud::OnFloatVTopic(const QString &_floatVTopic) { std::lock_guard lock(this->dataPtr->mutex); + // Unsubscribe from previous choice if (!this->dataPtr->floatVTopic.empty() && !this->dataPtr->node.Unsubscribe(this->dataPtr->floatVTopic)) @@ -208,6 +216,7 @@ void PointCloud::OnFloatVTopic(const QString &_floatVTopic) return; } gzmsg << "Subscribed to " << this->dataPtr->floatVTopic << std::endl; + this->dataPtr->hasFloatTopic = true; } ////////////////////////////////////////////////// @@ -411,30 +420,107 @@ void PointCloud::Implementation::PublishMarkers() gzwarn << "Mal-formatted pointcloud" << std::endl; } - for (; ptIdx < std::min(this->floatVMsg.data().size(), num_points); - ++iterX, ++iterY, ++iterZ, ++ptIdx) + if (this->hasFloatTopic) + { + for (; ptIdx < std::min(this->floatVMsg.data().size(), num_points); + ++iterX, ++iterY, ++iterZ, ++ptIdx) + { + // Value from float vector, if available. Otherwise publish all data as + // zeroes. + float dataVal = this->floatVMsg.data(ptIdx); + + // Don't visualize NaN + if (std::isnan(dataVal)) + continue; + + auto ratio = floatRange > 0 ? + (dataVal - this->minFloatV) / floatRange : 0.0f; + gz:: math::Color color{ + minC.R() + (maxC.R() - minC.R()) * ratio, + minC.G() + (maxC.G() - minC.G()) * ratio, + minC.B() + (maxC.B() - minC.B()) * ratio + }; + + gz::msgs::Set(marker.add_materials()->mutable_diffuse(), color); + gz::msgs::Set(marker.add_point(), gz::math::Vector3d( + *iterX, + *iterY, + *iterZ)); + } + } + else { - // Value from float vector, if available. Otherwise publish all data as - // zeroes. - float dataVal = this->floatVMsg.data(ptIdx); - - // Don't visualize NaN - if (std::isnan(dataVal)) - continue; - - auto ratio = floatRange > 0 ? - (dataVal - this->minFloatV) / floatRange : 0.0f; - gz:: math::Color color{ - minC.R() + (maxC.R() - minC.R()) * ratio, - minC.G() + (maxC.G() - minC.G()) * ratio, - minC.B() + (maxC.B() - minC.B()) * ratio - }; - - gz::msgs::Set(marker.add_materials()->mutable_diffuse(), color); - gz::msgs::Set(marker.add_point(), gz::math::Vector3d( - *iterX, - *iterY, - *iterZ)); + // Fall back to coloring using the point cloud (if possible) + // Else fall back to a default color + std::string r_detected, g_detected, b_detected; + for (auto field : this->pointCloudMsg.field()) + { + if (field.name() == "r" || field.name() == "red") + { + if (field.datatype() != msgs::PointCloudPacked::Field::UINT8) + { + gzwarn << "Only 256 bit color supported" << std::endl; + continue; + } + r_detected = field.name(); + } + + if (field.name() == "g" || field.name() == "green") + { + if (field.datatype() != msgs::PointCloudPacked::Field::UINT8) + { + gzwarn << "Only 256 bit color supported" << std::endl; + continue; + } + g_detected = field.name(); + } + + if (field.name() == "b" || field.name() == "blue") + { + if (field.datatype() != msgs::PointCloudPacked::Field::UINT8) + { + gzwarn << "Only 256 bit color supported" << std::endl; + continue; + } + b_detected = field.name(); + } + } + + if (r_detected.empty() || g_detected.empty() || b_detected.empty()) + { + gzerr << "Using default color" << std::endl; + // Color fields unavailable just set the color based on our max color + for (std::size_t i = 0; i < num_points; ++iterX, ++iterY, ++iterZ, ++i) + { + gz::msgs::Set(marker.add_materials()->mutable_diffuse(), minC); + gz::msgs::Set(marker.add_point(), gz::math::Vector3d( + *iterX, + *iterY, + *iterZ)); + } + } + else + { + gz::msgs::PointCloudPackedIterator + iterR(this->pointCloudMsg, r_detected); + gz::msgs::PointCloudPackedIterator + iterG(this->pointCloudMsg, g_detected); + gz::msgs::PointCloudPackedIterator + iterB(this->pointCloudMsg, b_detected); + for (std::size_t i = 0; i < num_points; + ++iterX, ++iterY, ++iterZ, ++i, ++iterR, ++iterG, ++iterB) + { + gz::msgs::Set(marker.add_materials()->mutable_diffuse(), math::Color( + static_cast(*iterR)/255.0, + static_cast(*iterG)/255.0, + static_cast(*iterB)/255.0 + )); + gz::msgs::Set(marker.add_point(), gz::math::Vector3d( + *iterX, + *iterY, + *iterZ)); + } + } } this->node.Request("/marker", marker);