EnvironmentPreload visualise static environments (gazebosim#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 `<ignore_time>` element is added to the EnvironmentPreload system xml it is ignored when visualising the point cloud data.

```xml
    <plugin
      filename="gz-sim-environment-preload-system"
      name="gz::sim::systems::EnvironmentPreload">
      <data>environmental_data.csv</data>
      <dimensions>
        <ignore_time>1</ignore_time>
        <time>timestamp</time>
        <space>
          <x>x</x>
          <y>y</y>
          <z>z</z>
        </space>
      </dimensions>
    </plugin>
``` 

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.
 
<img width="1312" height="940" alt="05-1-gz-wind-pc-x-axis" src="https://github.com/user-attachments/assets/72d9bd99-3640-456d-975a-9fc2d6387884" />


---------

Signed-off-by: Rhys Mainwaring <[email protected]>

Author: srmainwaring

examples/worlds/environmental_sensor.sdf

@@ -36,6 +36,7 @@
       name="gz::sim::systems::EnvironmentPreload">
       <data>environmental_data.csv</data>
       <dimensions>
+        <ignore_time>0</ignore_time>
         <time>timestamp</time>
         <space>
           <x>x</x>

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<std::mutex> 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());
   }

src/systems/environment_preload/VisualizationTool.cc

@@ -37,8 +37,11 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics(
     this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
     gz::msgs::Float_V msg;
     this->floatFields.emplace(key, msg);
+
     const double time = std::chrono::duration<double>(_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,21 +56,30 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics(
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::FileReloaded()
 {
+  std::lock_guard<std::mutex> lock(this->mutex);
   this->finishedTime = false;
 }
 
+/////////////////////////////////////////////////
+void EnvironmentVisualizationTool::Resample()
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  this->resample = true;
+}
+
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::Step(
     const UpdateInfo &_info,
     const EntityComponentManager &_ecm,
     const std::shared_ptr<components::EnvironmentalData> &_data,
     unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples)
 {
+  std::lock_guard<std::mutex> 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<double> 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<double>(_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<double>(_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<std::size_t>(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<float> xIter(pcMsg, "x");
@@ -199,22 +213,23 @@ 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())
         {
           continue;
         }
 
-        auto pos = coords.value();
+        const auto pos = coords.value();
         *xIter = pos.X();
         *yIter = pos.Y();
         *zIter = pos.Z();

src/systems/environment_preload/VisualizationTool.hh

@@ -62,7 +62,7 @@ class EnvironmentVisualizationTool
   private: bool first{true};
 
   /// \brief Enable resampling
-  public: std::atomic<bool> 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