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