From 9706ec2613433cdd61665480100eb1913eb9bc85 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) --- examples/worlds/environmental_sensor.sdf | 1 + .../environment_preload/EnvironmentPreload.cc | 8 +- .../environment_preload/VisualizationTool.cc | 97 +++++++++++-------- .../environment_preload/VisualizationTool.hh | 5 +- 4 files changed, 65 insertions(+), 46 deletions(-) diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf index 297f9db5f0..ce9eccacca 100644 --- a/examples/worlds/environmental_sensor.sdf +++ b/examples/worlds/environmental_sensor.sdf @@ -36,6 +36,7 @@ name="gz::sim::systems::EnvironmentPreload"> environmental_data.csv + 0 x diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 3aa0ca4e7a..d6479687f1 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -120,7 +120,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate } this->samples = converted; this->visualize = true; - this->visualizationPtr->resample = true; + this->visualizationPtr->Resample(); } ////////////////////////////////////////////////// @@ -282,7 +282,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); - this->visualizationPtr->resample = true; + this->visualizationPtr->Resample(); this->fileLoaded = true; } catch (const std::invalid_argument &exc) @@ -337,7 +337,7 @@ void EnvironmentPreload::PreUpdate( scopedName(world, _ecm) + "/environment/visualize/res"), &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); - this->dataPtr->visualizationPtr->resample = true; + this->dataPtr->visualizationPtr->Resample(); this->dataPtr->ReadSdf(_ecm); } @@ -349,7 +349,7 @@ void EnvironmentPreload::PreUpdate( if (this->dataPtr->visualize) { std::lock_guard lock(this->dataPtr->mtx); - auto samples = this->dataPtr->samples; + const auto &samples = this->dataPtr->samples; this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, samples.X(), samples.Y(), samples.Z()); } diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc index 6498231541..16013901d3 100644 --- a/src/systems/environment_preload/VisualizationTool.cc +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -37,8 +37,11 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics( 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(); - auto sess = _data->frame[key].CreateSession(time); + 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" @@ -53,9 +56,17 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics( ///////////////////////////////////////////////// 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, @@ -63,11 +74,12 @@ void EnvironmentVisualizationTool::Step( const std::shared_ptr &_data, unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { + std::lock_guard lock(this->mutex); if (this->finishedTime) { return; } - auto now = std::chrono::steady_clock::now(); + const auto now = std::chrono::steady_clock::now(); std::chrono::duration dt(now - this->lastTick); if (this->resample) @@ -82,23 +94,25 @@ void EnvironmentVisualizationTool::Step( this->lastTick = now; } - // Progress session pointers to next time stamp - for (auto &it : this->sessions) + if (!_data->staticTime) { - auto res = - _data->frame[it.first].StepTo(it.second, - std::chrono::duration(_info.simTime).count()); - if (res.has_value()) - { - it.second = res.value(); - } - else + // Progress session pointers to next time stamp + for (auto &it : this->sessions) { - gzerr << "Data does not exist beyond this time." - << " Not publishing new environment visualization data." - << std::endl; - this->finishedTime = true; - return; + 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; + } } } @@ -118,24 +132,24 @@ void EnvironmentVisualizationTool::Visualize( { for (auto key : _data->frame.Keys()) { - const auto session = this->sessions[key]; - auto frame = _data->frame[key]; - auto [lower_bound, upper_bound] = frame.Bounds(session); - auto step = upper_bound - lower_bound; - auto dx = step.X() / _xSamples; - auto dy = step.Y() / _ySamples; - auto dz = step.Z() / _zSamples; + 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++) { - auto x = lower_bound.X() + x_steps * dx; + const auto x = lower_bound.X() + x_steps * dx; for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) { - auto y = lower_bound.Y() + y_steps * dy; + const auto y = lower_bound.Y() + y_steps * dy; for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) { - auto z = lower_bound.Z() + z_steps * dz; - auto res = frame.LookUp(session, math::Vector3d(x, y, z)); + const auto z = lower_bound.Z() + z_steps * dz; + const auto res = frame.LookUp(session, math::Vector3d(x, y, z)); if (res.has_value()) { @@ -176,21 +190,21 @@ void EnvironmentVisualizationTool::ResizeCloud( // Assume all data have same point cloud. gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; + 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); - auto session = this->sessions[this->pubs.begin()->first]; - auto frame = _data->frame[this->pubs.begin()->first]; - auto [lower_bound, upper_bound] = frame.Bounds(session); + 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); - auto step = upper_bound - lower_bound; - auto dx = step.X() / _numXSamples; - auto dy = step.Y() / _numYSamples; - auto dz = step.Z() / _numZSamples; + 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"); @@ -199,14 +213,15 @@ void EnvironmentVisualizationTool::ResizeCloud( for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) { - auto x = lower_bound.X() + x_steps * dx; + const auto x = lower_bound.X() + x_steps * dx; for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) { - auto y = lower_bound.Y() + y_steps * dy; + const auto y = lower_bound.Y() + y_steps * dy; for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) { - auto z = lower_bound.Z() + z_steps * dz; - auto coords = getGridFieldCoordinates(_ecm, math::Vector3d{x, y, z}, + const auto z = lower_bound.Z() + z_steps * dz; + const auto coords = getGridFieldCoordinates( + _ecm, math::Vector3d{x, y, z}, _data); if (!coords.has_value()) @@ -214,7 +229,7 @@ void EnvironmentVisualizationTool::ResizeCloud( continue; } - auto pos = coords.value(); + const auto pos = coords.value(); *xIter = pos.X(); *yIter = pos.Y(); *zIter = pos.Z(); diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh index 06505c042b..2214c086b3 100644 --- a/src/systems/environment_preload/VisualizationTool.hh +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -62,7 +62,7 @@ class EnvironmentVisualizationTool private: bool first{true}; /// \brief Enable resampling - public: std::atomic resample{true}; + private: bool resample{true}; /// \brief Time has come to an end. private: bool finishedTime{false}; @@ -78,6 +78,9 @@ class EnvironmentVisualizationTool /// \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