From 13d15c39a2fbf45cdb1f610605b8f24af3165108 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 22 Oct 2025 16:16:46 +0100 Subject: [PATCH] EnvironmentPreload visualise static environments (#3138) ## Summary This PR fixes an issue where a static environment loaded by the EnvironmentPreload system would not display using the Environment Visualization Resolution and Point Cloud GUI widgets. There is also a fix that prevents segmentation faults in the VisualizationTool caused by copying a DataFrame which may invalidate the session iterators. ## Details Before this change, if the `` element is added to the EnvironmentPreload system xml it is ignored when visualising the point cloud data. ```xml environmental_data.csv 1 x y z ``` This means that if only the time zero data is provided, for example: ```bash # environmental_data.csv - modified to only contain data for timestamp = 0 timestamp,humidity,x,y,z 0,80,-1,-1,-1 0,80,-1,-1, 1 0,80,-1, 1,-1 0,80,-1, 1, 1 0,90, 1,-1,-1 0,90, 1,-1, 1 0,90, 1, 1,-1 0,90, 1, 1, 1 ``` the VisualizationTool would never display the point cloud data. A workaround for this is to set a very large timestamp for a second copy of data. This is unsatisfactory because it doubles the size of the environment data required (which may be significant for large vector fields), and also requires a temporal interpolation when none is required, which results in a performance penalty. The other fix is to pay attention to unintended copies when using the auto keyword to obtain aliases to the data frame objects. The main change is ```diff - auto frame = _data->frame[this->pubs.begin()->first]; + const auto &frame = _data->frame[this->pubs.begin()->first]; ``` The problem is that the session iterators will in general be invalid for the copied frame, and this may result in a segmentation fault when they are dereferenced later in the code. In this particular case the check that the iterator was in range in the interpolation code failed resulting in a out of range access violation a few lines later. This type of error can be difficult to trace. With this change, and some other related fixes to the environmental data system, large static vector fields can be loaded and viewed in the GUI. Below is an example for a wind field containing 64000 rows of data. 05-1-gz-wind-pc-x-axis --------- Signed-off-by: Rhys Mainwaring (cherry picked from commit bddeeeade0006ad43677df3135ac5e55acf70784) # Conflicts: # examples/worlds/environmental_sensor.sdf # src/systems/environment_preload/EnvironmentPreload.cc # src/systems/environment_preload/VisualizationTool.cc # src/systems/environment_preload/VisualizationTool.hh --- examples/worlds/environmental_sensor.sdf | 131 +++++++ .../environment_preload/EnvironmentPreload.cc | 363 ++++++++++++++++++ .../environment_preload/VisualizationTool.cc | 247 ++++++++++++ .../environment_preload/VisualizationTool.hh | 146 +++++++ 4 files changed, 887 insertions(+) create mode 100644 examples/worlds/environmental_sensor.sdf create mode 100644 src/systems/environment_preload/EnvironmentPreload.cc create mode 100644 src/systems/environment_preload/VisualizationTool.cc create mode 100644 src/systems/environment_preload/VisualizationTool.hh diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf new file mode 100644 index 0000000000..ce9eccacca --- /dev/null +++ b/examples/worlds/environmental_sensor.sdf @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + environmental_data.csv + + 0 + + + x + y + z + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.05 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1 + 30 + sensors/humidity + + + + + + diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc new file mode 100644 index 0000000000..d6479687f1 --- /dev/null +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -0,0 +1,363 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include "EnvironmentPreload.hh" +#include "VisualizationTool.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +using Units = msgs::DataLoadPathOptions_DataAngularUnits; +////////////////////////////////////////////////// +class gz::sim::systems::EnvironmentPreloadPrivate +{ + /// \brief Is the file loaded + public: bool loaded{false}; + + /// \brief SDF Description + public: std::shared_ptr sdf; + + /// \brief GzTransport node + public: transport::Node node; + + /// \brief Data descriptions + public: msgs::DataLoadPathOptions dataDescription; + + /// \brief mutex to protect the samples and data description + public: std::mutex mtx; + + /// \brief Do we need to reload the system. + public: std::atomic needsReload{false}; + + /// \brief Visualization Helper + public: std::unique_ptr visualizationPtr; + + /// \brief Are visualizations enabled + public: bool visualize{false}; + + /// \brief Sample resolutions + public: math::Vector3 samples; + + /// \brief Is the file loaded + public: bool fileLoaded{false}; + + /// \brief File loading error logger + public: bool logFileLoadError{true}; + + /// \brief Reference to data + public: std::shared_ptr envData; + + ////////////////////////////////////////////////// + public: EnvironmentPreloadPrivate() : + visualizationPtr(new EnvironmentVisualizationTool) {} + + ////////////////////////////////////////////////// + public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg) + { + std::lock_guard lock(this->mtx); + this->dataDescription = _msg; + this->needsReload = true; + this->logFileLoadError = true; + this->visualizationPtr->FileReloaded(); + gzdbg << "Loading file " << _msg.path() << "\n"; + } + + ////////////////////////////////////////////////// + public: void OnVisualResChanged(const msgs::Vector3d &_resChanged) + { + std::lock_guard lock(this->mtx); + if (!this->fileLoaded) + { + // Only visualize if a file exists + return; + } + math::Vector3 converted{ + static_cast(ceil(_resChanged.x())), + static_cast(ceil(_resChanged.y())), + static_cast(ceil(_resChanged.z()))}; + if (this->samples.X() == converted.X() && + this->samples.Y() == converted.Y() && + this->samples.Z() == converted.Z()) + { + // If the sample has not changed return. + // This is because resampling is expensive. + return; + } + this->samples = converted; + this->visualize = true; + this->visualizationPtr->Resample(); + } + + ////////////////////////////////////////////////// + public: void ReadSdf(EntityComponentManager &_ecm) + { + if (!this->sdf->HasElement("data")) + { + gzerr << "No environmental data file was specified" << std::endl; + return; + } + + std::lock_guard lock(mtx); + std::string dataPath = + this->sdf->Get("data"); + + if (common::isRelativePath(dataPath)) + { + auto *component = + _ecm.Component(worldEntity(_ecm)); + const std::string rootPath = + common::parentPath(component->Data().Element()->FilePath()); + dataPath = common::joinPaths(rootPath, dataPath); + } + this->dataDescription.set_path(dataPath); + + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_RADIANS); + std::string timeColumnName{"t"}; + bool ignoreTime = false; + std::array spatialColumnNames{"x", "y", "z"}; + sdf::ElementConstPtr elem = + this->sdf->FindElement("dimensions"); + msgs::SphericalCoordinatesType spatialReference = + msgs::SphericalCoordinatesType::GLOBAL; + if (elem) + { + if (elem->HasElement("ignore_time")) + { + ignoreTime = elem->Get("ignore_time"); + } + if (elem->HasElement("time")) + { + timeColumnName = elem->Get("time"); + } + elem = elem->FindElement("space"); + if (elem) + { + if (elem->HasAttribute("reference")) + { + const std::string referenceName = + elem->Get("reference"); + if (referenceName == "global") + { + spatialReference = msgs::SphericalCoordinatesType::GLOBAL; + } + else if (referenceName == "spherical") + { + spatialReference = msgs::SphericalCoordinatesType::SPHERICAL; + if (elem->HasAttribute("units")) + { + std::string unitName = elem->Get("units"); + if (unitName == "degrees") + { + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_DEGREES); + } + else if (unitName != "radians") + { + gzerr << "Unrecognized unit " << unitName << "\n"; + } + } + } + else if (referenceName == "ecef") + { + spatialReference = msgs::SphericalCoordinatesType::ECEF; + } + else + { + gzerr << "Unknown reference '" << referenceName << "'" + << std::endl; + return; + } + } + for (size_t i = 0; i < spatialColumnNames.size(); ++i) + { + if (elem->HasElement(spatialColumnNames[i])) + { + spatialColumnNames[i] = + elem->Get(spatialColumnNames[i]); + } + } + } + } + + this->dataDescription.set_static_time(ignoreTime); + this->dataDescription.set_coordinate_type(spatialReference); + this->dataDescription.set_time(timeColumnName); + this->dataDescription.set_x(spatialColumnNames[0]); + this->dataDescription.set_y(spatialColumnNames[1]); + this->dataDescription.set_z(spatialColumnNames[2]); + + this->needsReload = true; + } + + ////////////////////////////////////////////////// + public: components::EnvironmentalData::ReferenceUnits ConvertUnits( + const Units &_unit) + { + switch (_unit) + { + case Units::DataLoadPathOptions_DataAngularUnits_DEGREES: + return components::EnvironmentalData::ReferenceUnits::DEGREES; + case Units::DataLoadPathOptions_DataAngularUnits_RADIANS: + return components::EnvironmentalData::ReferenceUnits::RADIANS; + default: + gzerr << "Invalid unit conversion. Defaulting to radians." << std::endl; + return components::EnvironmentalData::ReferenceUnits::RADIANS; + } + } + + ////////////////////////////////////////////////// + public: void LoadEnvironment(EntityComponentManager &_ecm) + { + try + { + std::lock_guard lock(this->mtx); + std::array spatialColumnNames{ + this->dataDescription.x(), + this->dataDescription.y(), + this->dataDescription.z()}; + + math::SphericalCoordinates::CoordinateType spatialReference = + msgs::Convert(this->dataDescription.coordinate_type()); + auto units = this->ConvertUnits(this->dataDescription.units()); + + std::ifstream dataFile(this->dataDescription.path()); + if (!dataFile.is_open()) + { + if (this->logFileLoadError) + { + gzerr << "No environmental data file was found at " << + this->dataDescription.path() << std::endl; + logFileLoadError = false; + } + return; + } + + gzmsg << "Loading Environment Data " << this->dataDescription.path() << + std::endl; + + using ComponentDataT = components::EnvironmentalData; + auto data = ComponentDataT::MakeShared( + common::IO::ReadFrom( + common::CSVIStreamIterator(dataFile), + common::CSVIStreamIterator(), + this->dataDescription.time(), spatialColumnNames), + spatialReference, units, this->dataDescription.static_time()); + this->envData = data; + using ComponentT = components::Environment; + auto component = ComponentT{std::move(data)}; + _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); + this->visualizationPtr->Resample(); + this->fileLoaded = true; + } + catch (const std::invalid_argument &exc) + { + if (this->logFileLoadError) + { + gzerr << "Failed to load environment data" << std::endl + << exc.what() << std::endl; + this->logFileLoadError = false; + } + } + + this->needsReload = false; + } +}; + +////////////////////////////////////////////////// +EnvironmentPreload::EnvironmentPreload() + : System(), dataPtr(new EnvironmentPreloadPrivate) +{ +} + +////////////////////////////////////////////////// +EnvironmentPreload::~EnvironmentPreload() = default; + +////////////////////////////////////////////////// +void EnvironmentPreload::Configure( + const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->sdf = _sdf; +} + +////////////////////////////////////////////////// +void EnvironmentPreload::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (!std::exchange(this->dataPtr->loaded, true)) + { + auto world = worldEntity(_ecm); + + // See https://github.com/gazebosim/gz-sim/issues/1786 + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment"), + &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get()); + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment/visualize/res"), + &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); + + this->dataPtr->visualizationPtr->Resample(); + this->dataPtr->ReadSdf(_ecm); + } + + if (this->dataPtr->needsReload) + { + this->dataPtr->LoadEnvironment(_ecm); + } + + if (this->dataPtr->visualize) + { + std::lock_guard lock(this->dataPtr->mtx); + const auto &samples = this->dataPtr->samples; + this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, + samples.X(), samples.Y(), samples.Z()); + } +} + +// Register this plugin +GZ_ADD_PLUGIN(EnvironmentPreload, System, + EnvironmentPreload::ISystemConfigure, + EnvironmentPreload::ISystemPreUpdate) +GZ_ADD_PLUGIN_ALIAS(EnvironmentPreload, + "gz::sim::systems::EnvironmentPreload") diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc new file mode 100644 index 0000000000..16013901d3 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -0,0 +1,247 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "VisualizationTool.hh" + +///////////////////////////////////////////////// +EnvironmentVisualizationTool::EnvironmentVisualizationTool() +{ + this->pcPub = + this->node.Advertise("/point_cloud"); +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info) +{ + this->pubs.clear(); + this->sessions.clear(); + + for (auto key : _data->frame.Keys()) + { + this->pubs.emplace(key, node.Advertise(key)); + gz::msgs::Float_V msg; + this->floatFields.emplace(key, msg); + + const double time = std::chrono::duration(_info.simTime).count(); + const auto sess = _data->staticTime ? + _data->frame[key].CreateSession(0.0) : + _data->frame[key].CreateSession(time); + if (!_data->frame[key].IsValid(sess)) + { + gzerr << key << "data is out of time bounds. Nothing will be published" + <finishedTime = true; + continue; + } + this->sessions.emplace(key, sess); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::FileReloaded() +{ + std::lock_guard lock(this->mutex); + this->finishedTime = false; +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Resample() +{ + std::lock_guard lock(this->mutex); + this->resample = true; +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) +{ + std::lock_guard lock(this->mutex); + if (this->finishedTime) + { + return; + } + const auto now = std::chrono::steady_clock::now(); + std::chrono::duration dt(now - this->lastTick); + + if (this->resample) + { + this->CreatePointCloudTopics(_data, _info); + if (this->finishedTime) { + this->resample = false; + return; + } + this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples); + this->resample = false; + this->lastTick = now; + } + + if (!_data->staticTime) + { + // Progress session pointers to next time stamp + for (auto &it : this->sessions) + { + const auto time = std::chrono::duration(_info.simTime).count(); + const auto res = _data->frame[it.first].StepTo(it.second, time); + if (res.has_value()) + { + it.second = res.value(); + } + else + { + gzerr << "Data does not exist beyond this time (t = " << time << ")." + << " Not publishing new environment visualization data." + << std::endl; + this->finishedTime = true; + return; + } + } + } + + // Publish at 2 hz for now. In future make reconfigureable. + if (dt.count() > 0.5 && !this->finishedTime) + { + this->Visualize(_data, _xSamples, _ySamples, _zSamples); + this->Publish(); + lastTick = now; + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Visualize( + const std::shared_ptr &_data, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) +{ + for (auto key : _data->frame.Keys()) + { + const auto &session = this->sessions[key]; + const auto &frame = _data->frame[key]; + const auto [lower_bound, upper_bound] = frame.Bounds(session); + const auto step = upper_bound - lower_bound; + const auto dx = step.X() / _xSamples; + const auto dy = step.Y() / _ySamples; + const auto dz = step.Z() / _zSamples; + std::size_t idx = 0; + for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++) + { + const auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) + { + const auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) + { + const auto z = lower_bound.Z() + z_steps * dz; + const auto res = frame.LookUp(session, math::Vector3d(x, y, z)); + + if (res.has_value()) + { + this->floatFields[key].mutable_data()->Set(idx, + static_cast(res.value())); + } + else + { + this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); + } + idx++; + } + } + } + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Publish() +{ + pcPub.Publish(this->pcMsg); + for (auto &[key, pub] : this->pubs) + { + pub.Publish(this->floatFields[key]); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _numXSamples, + unsigned int _numYSamples, + unsigned int _numZSamples) +{ + assert(pubs.size() > 0); + + // Assume all data have same point cloud. + gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, + {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); + const auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; + std::size_t dataSize{ + static_cast(numberOfPoints * pcMsg.point_step())}; + pcMsg.mutable_data()->resize(dataSize); + pcMsg.set_height(1); + pcMsg.set_width(numberOfPoints); + + const auto &session = this->sessions[this->pubs.begin()->first]; + const auto &frame = _data->frame[this->pubs.begin()->first]; + const auto [lower_bound, upper_bound] = frame.Bounds(session); + + const auto step = upper_bound - lower_bound; + const auto dx = step.X() / _numXSamples; + const auto dy = step.Y() / _numYSamples; + const auto dz = step.Z() / _numZSamples; + + // Populate point cloud + gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) + { + const auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) + { + const auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) + { + const auto z = lower_bound.Z() + z_steps * dz; + const auto coords = getGridFieldCoordinates( + _ecm, math::Vector3d{x, y, z}, + _data); + + if (!coords.has_value()) + { + continue; + } + + const auto pos = coords.value(); + *xIter = pos.X(); + *yIter = pos.Y(); + *zIter = pos.Z(); + ++xIter; + ++yIter; + ++zIter; + } + } + } + for (auto key : _data->frame.Keys()) + { + this->floatFields[key].mutable_data()->Resize( + numberOfPoints, std::nanf("")); + } +} diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh new file mode 100644 index 0000000000..2214c086b3 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ +#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + +/// \brief This class helps handle point cloud visuallizations +/// of environment data. +class EnvironmentVisualizationTool +{ + /// \brief Environment constructor + public: EnvironmentVisualizationTool(); + + /// \brief To synchronize member access. + private: std::mutex mutex; + + /// \brief First load we need to scan for existing data sensor + private: bool first{true}; + + /// \brief Enable resampling + private: bool resample{true}; + + /// \brief Time has come to an end. + private: bool finishedTime{false}; + + /// \brief Create publisher structures whenever a new environment is made + /// available. + /// \param[in] _data Data to be visualized + /// \param[in] _info simulation info for current time step + private: void CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info); + + /// \brief Invoke when new file is made available. + public: void FileReloaded(); + + /// \brief Invoke when new file is made available. + public: void Resample(); + + /// \brief Step the visualizations + /// \param[in] _info The simulation info including timestep + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visualized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + public: void Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publishes a sample of the data + /// \param[in] _data The data to be visualized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void Visualize( + const std::shared_ptr &_data, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Get the point cloud data. + private: void Publish(); + + /// \brief Resize the point cloud structure (used to reallocate + /// memory when resolution changes) + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visualized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publisher for point clouds + private: transport::Node::Publisher pcPub; + + /// \brief Publishers for data + private: std::unordered_map pubs; + + /// \brief Floating point message buffers + private: std::unordered_map floatFields; + + /// \brief GZ buffers + private: transport::Node node; + + /// \brief Point cloud buffer + private: gz::msgs::PointCloudPacked pcMsg; + + /// \brief Session cursors + private: std::unordered_map> sessions; + + /// \brief Duration from last update + private: std::chrono::time_point lastTick; +}; +} +} +} +#endif