From ee16b8d407b695852a161733a6d9dae6d7fefb58 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 20 Dec 2022 22:07:47 +0800
Subject: [PATCH 01/28] Fix enviroment system loading mechanism

Currently, there is an issue with the way the Environment loader plugin loads data. In particular it directly writes to the ECM. While this makes sense intuitively, it does not work in practice as the GUI runs on a client process while systems that use it run on the server. This PR fixes this issue by introducing a topic through which the GUI may load Environment Data on the server.

TODO:
- [ ] This commit breaks the visuallization plugin. Fix it so the visuallization plugin runs the data sampling off the server plugin.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../environment_loader/EnvironmentLoader.cc   |  77 ++++----
 .../environment_preload/EnvironmentPreload.cc | 169 +++++++++++++-----
 2 files changed, 163 insertions(+), 83 deletions(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index 6ac8b464cfd..6f00178167c 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -19,7 +19,6 @@
 
 #include <gz/gui/Application.hh>
 #include <gz/gui/MainWindow.hh>
-#include <gz/sim/components/Environment.hh>
 #include <gz/sim/Util.hh>
 
 #include <gz/plugin/Register.hh>
@@ -33,6 +32,11 @@
 #include <gz/common/CSVStreams.hh>
 #include <gz/common/DataFrame.hh>
 
+#include <gz/transport/Node.hh>
+
+#include <gz/msgs/data_load_options.pb.h>
+#include <gz/msgs/Utility.hh>
+
 using namespace gz;
 using namespace sim;
 
@@ -42,6 +46,8 @@ namespace sim
 {
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
+
+using Units = msgs::DataLoadPathOptions_DataAngularUnits;
 /// \brief Private data class for EnvironmentLoader
 class EnvironmentLoaderPrivate
 {
@@ -65,7 +71,7 @@ class EnvironmentLoaderPrivate
   public: int zIndex{-1};
 
   /// \brief Index of data dimension to be used as units.
-  public: QString unit;
+  public: QString unit{"radians"};
 
   public: using ReferenceT = math::SphericalCoordinates::CoordinateType;
 
@@ -76,12 +82,12 @@ class EnvironmentLoaderPrivate
     {QString("ecef"), math::SphericalCoordinates::ECEF}};
 
   /// \brief Map of supported spatial units.
-  public: const QMap<QString, components::EnvironmentalData::ReferenceUnits>
+  public: const QMap<QString, Units>
     unitMap{
       {QString("degree"),
-        components::EnvironmentalData::ReferenceUnits::DEGREES},
+        Units::DataLoadPathOptions_DataAngularUnits_DEGREES},
       {QString("radians"),
-        components::EnvironmentalData::ReferenceUnits::RADIANS}
+        Units::DataLoadPathOptions_DataAngularUnits_DEGREES}
     };
 
   /// \brief Spatial reference.
@@ -92,6 +98,12 @@ class EnvironmentLoaderPrivate
 
   /// \brief Whether to attempt an environmental data load.
   public: std::atomic<bool> needsLoad{false};
+
+  /// \brief Gz transport node
+  public: transport::Node node;
+
+  /// \brief publisher
+  public: std::optional<transport::Node::Publisher> pub{std::nullopt};
 };
 }
 }
@@ -123,46 +135,35 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *)
 void EnvironmentLoader::Update(const UpdateInfo &,
                                EntityComponentManager &_ecm)
 {
-  if (this->dataPtr->needsLoad)
+  if (!this->dataPtr->pub.has_value())
   {
-    std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
-    this->dataPtr->needsLoad = false;
-
-    /// TODO(arjo): Handle the case where we are loading a file in windows.
-    /// Should SDFormat files support going from windows <=> unix paths?
-    std::ifstream dataFile(this->dataPtr->dataPath.toStdString());
-    gzmsg << "Loading environmental data from "
-          << this->dataPtr->dataPath.toStdString()
-          << std::endl;
-    try
-    {
-      using ComponentDataT = components::EnvironmentalData;
-      auto data = ComponentDataT::MakeShared(
-          common::IO<ComponentDataT::FrameT>::ReadFrom(
-              common::CSVIStreamIterator(dataFile),
-              common::CSVIStreamIterator(),
-              this->dataPtr->timeIndex, {
-                static_cast<size_t>(this->dataPtr->xIndex),
-                static_cast<size_t>(this->dataPtr->yIndex),
-                static_cast<size_t>(this->dataPtr->zIndex)}),
-          this->dataPtr->referenceMap[this->dataPtr->reference],
-          this->dataPtr->unitMap[this->dataPtr->unit]);
-
-      using ComponentT = components::Environment;
-      _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)});
-    }
-    catch (const std::invalid_argument &exc)
-    {
-      gzerr << "Failed to load environmental data" << std::endl
-            << exc.what() << std::endl;
-    }
+    auto world = worldEntity(_ecm);
+    auto topic = common::joinPaths(scopedName(world, _ecm),"environment");
+    this->dataPtr->pub =
+      {this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
   }
 }
 
 /////////////////////////////////////////////////
 void EnvironmentLoader::ScheduleLoad()
 {
-  this->dataPtr->needsLoad = this->IsConfigured();
+  if(this->IsConfigured() && this->dataPtr->pub.has_value())
+  {
+    msgs::DataLoadPathOptions data;
+    data.set_path(this->dataPtr->dataPath.toStdString());
+    data.set_time(
+      this->dataPtr->dimensionList[this->dataPtr->timeIndex].toStdString());
+    data.set_x(
+      this->dataPtr->dimensionList[this->dataPtr->xIndex].toStdString());
+    data.set_y(
+      this->dataPtr->dimensionList[this->dataPtr->yIndex].toStdString());
+    data.set_z(
+      this->dataPtr->dimensionList[this->dataPtr->zIndex].toStdString());
+    auto referenceFrame = this->dataPtr->referenceMap[this->dataPtr->reference];
+
+    data.set_coordinate_type(msgs::ConvertCoord(referenceFrame));
+    data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]);
+  }
 }
 
 /////////////////////////////////////////////////
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index fe5b57f940c..38c057e3bd2 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -27,6 +27,11 @@
 
 #include <gz/plugin/Register.hh>
 
+#include <gz/transport/Node.hh>
+
+#include <gz/msgs/data_load_options.pb.h>
+#include <gz/msgs/Utility.hh>
+
 #include "gz/sim/components/Environment.hh"
 #include "gz/sim/components/World.hh"
 #include "gz/sim/Util.hh"
@@ -35,72 +40,59 @@ using namespace gz;
 using namespace sim;
 using namespace systems;
 
+using Units = msgs::DataLoadPathOptions_DataAngularUnits;
 //////////////////////////////////////////////////
 class gz::sim::systems::EnvironmentPreloadPrivate
 {
   public: bool loaded{false};
 
   public: std::shared_ptr<const sdf::Element> sdf;
-};
 
-//////////////////////////////////////////////////
-EnvironmentPreload::EnvironmentPreload()
-  : System(), dataPtr(new EnvironmentPreloadPrivate)
-{
-}
+  public: transport::Node node;
 
-//////////////////////////////////////////////////
-EnvironmentPreload::~EnvironmentPreload() = default;
+  public: msgs::DataLoadPathOptions dataDescription;
 
-//////////////////////////////////////////////////
-void EnvironmentPreload::Configure(
-    const Entity &/*_entity*/,
-    const std::shared_ptr<const sdf::Element> &_sdf,
-    EntityComponentManager &/*_ecm*/,
-    EventManager &/*_eventMgr*/)
-{
-  this->dataPtr->sdf = _sdf;
-}
+  public: std::mutex mtx;
 
-//////////////////////////////////////////////////
-void EnvironmentPreload::PreUpdate(
-  const gz::sim::UpdateInfo &,
-  gz::sim::EntityComponentManager &_ecm)
-{
-  if (!std::exchange(this->dataPtr->loaded, true))
+  public: std::atomic<bool> needsReload{false};
+
+  public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg)
   {
-    if (!this->dataPtr->sdf->HasElement("data"))
+    std::lock_guard<std::mutex> lock(this->mtx);
+    this->dataDescription = _msg;
+    this->needsReload = true;
+  }
+
+  public: void ReadSdf(EntityComponentManager &_ecm)
+  {
+    if (!this->sdf->HasElement("data"))
     {
-      gzerr << "No environmental data file was specified";
+      gzwarn << "No environmental data file was specified";
       return;
     }
 
+    std::lock_guard<std::mutex> lock(mtx);
     std::string dataPath =
-        this->dataPtr->sdf->Get<std::string>("data");
-    if (common::isRelativePath(dataPath))
+        this->sdf->Get<std::string>("data");
+    this->dataDescription.set_path(dataPath);
+    if (common::isRelativePath(dataDescription.path()))
     {
       auto * component =
           _ecm.Component<components::WorldSdf>(worldEntity(_ecm));
       const std::string rootPath =
           common::parentPath(component->Data().Element()->FilePath());
-      dataPath = common::joinPaths(rootPath, dataPath);
-    }
-
-    std::ifstream dataFile(dataPath);
-    if (!dataFile.is_open())
-    {
-      gzerr << "No environmental data file was found at " << dataPath;
-      return;
+      dataPath = common::joinPaths(rootPath, this->dataDescription.path());
     }
 
-    components::EnvironmentalData::ReferenceUnits unit{
-      components::EnvironmentalData::ReferenceUnits::RADIANS};
+    this->dataDescription.set_units(
+      Units::DataLoadPathOptions_DataAngularUnits_RADIANS);
     std::string timeColumnName{"t"};
     bool ignoreTime = false;
     std::array<std::string, 3> spatialColumnNames{"x", "y", "z"};
-    auto spatialReference = math::SphericalCoordinates::GLOBAL;
     sdf::ElementConstPtr elem =
-        this->dataPtr->sdf->FindElement("dimensions");
+        this->sdf->FindElement("dimensions");
+    msgs::SphericalCoordinatesType spatialReference =
+      msgs::SphericalCoordinatesType::GLOBAL;
     if (elem)
     {
       if (elem->HasElement("ignore_time"))
@@ -120,17 +112,18 @@ void EnvironmentPreload::PreUpdate(
               elem->Get<std::string>("reference");
           if (referenceName == "global")
           {
-            spatialReference = math::SphericalCoordinates::GLOBAL;
+            spatialReference = msgs::SphericalCoordinatesType::GLOBAL;
           }
           else if (referenceName == "spherical")
           {
-            spatialReference = math::SphericalCoordinates::SPHERICAL;
+            spatialReference = msgs::SphericalCoordinatesType::SPHERICAL;
             if (elem->HasAttribute("units"))
             {
               std::string unitName = elem->Get<std::string>("units");
               if (unitName == "degrees")
               {
-                unit = components::EnvironmentalData::ReferenceUnits::DEGREES;
+                this->dataDescription.set_units(
+                  Units::DataLoadPathOptions_DataAngularUnits_DEGREES);
               }
               else if (unitName != "radians")
               {
@@ -140,7 +133,7 @@ void EnvironmentPreload::PreUpdate(
           }
           else if (referenceName == "ecef")
           {
-            spatialReference = math::SphericalCoordinates::ECEF;
+            spatialReference = msgs::SphericalCoordinatesType::ECEF;
           }
           else
           {
@@ -160,16 +153,57 @@ void EnvironmentPreload::PreUpdate(
       }
     }
 
+    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]);
+
+    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;
+    }
+  }
+
+  public: void LoadEnvironment(EntityComponentManager &_ecm)
+  {
     try
     {
+      std::lock_guard<std::mutex> lock(this->mtx);
+      std::array<std::string, 3> 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())
+      {
+        gzerr << "No environmental data file was found at " <<
+          this->dataDescription.path();
+        return;
+      }
+
       gzmsg << "Loading Environment Data\n";
       using ComponentDataT = components::EnvironmentalData;
       auto data = ComponentDataT::MakeShared(
           common::IO<ComponentDataT::FrameT>::ReadFrom(
               common::CSVIStreamIterator(dataFile),
               common::CSVIStreamIterator(),
-              timeColumnName, spatialColumnNames),
-          spatialReference, unit, ignoreTime);
+              this->dataDescription.time(), spatialColumnNames),
+          spatialReference, units, this->dataDescription.static_time());
 
       using ComponentT = components::Environment;
       auto component = ComponentT{std::move(data)};
@@ -180,6 +214,51 @@ void EnvironmentPreload::PreUpdate(
       gzerr << "Failed to load environment data" << std::endl
             << exc.what() << std::endl;
     }
+
+    needsReload = false;
+  }
+};
+
+//////////////////////////////////////////////////
+EnvironmentPreload::EnvironmentPreload()
+  : System(), dataPtr(new EnvironmentPreloadPrivate)
+{
+}
+
+//////////////////////////////////////////////////
+EnvironmentPreload::~EnvironmentPreload() = default;
+
+//////////////////////////////////////////////////
+void EnvironmentPreload::Configure(
+    const Entity &/*_entity*/,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &/*_ecm*/,
+    EventManager &/*_eventMgr*/)
+{
+  this->dataPtr->sdf = _sdf;
+}
+
+//////////////////////////////////////////////////
+void EnvironmentPreload::PreUpdate(
+  const gz::sim::UpdateInfo &,
+  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(
+      common::joinPaths(scopedName(world, _ecm),"environment"),
+      &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get());
+      msgs::DataLoadPathOptions loadOptions;
+
+    this->dataPtr->ReadSdf(_ecm);
+  }
+
+  if (this->dataPtr->needsReload.load())
+  {
+    this->dataPtr->LoadEnvironment(_ecm);
   }
 }
 

From 2ce77fcd44874c6b3fe515aa9803ed76b48935c7 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 21 Dec 2022 22:38:18 +0800
Subject: [PATCH 02/28] small changes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 src/systems/environment_preload/EnvironmentPreload.cc | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 38c057e3bd2..0c449b966e2 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -171,6 +171,9 @@ class gz::sim::systems::EnvironmentPreloadPrivate
         return components::EnvironmentalData::ReferenceUnits::DEGREES;
       case Units::DataLoadPathOptions_DataAngularUnits_RADIANS:
         return components::EnvironmentalData::ReferenceUnits::RADIANS;
+      default:
+        gzerr << "Invalid unit conversion" << std::endl;
+        return components::EnvironmentalData::ReferenceUnits::RADIANS;
     }
   }
 

From 38297242072be213c1da6423489151ab807331e4 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 12 Jan 2023 08:56:32 +0800
Subject: [PATCH 03/28] Working on porting the visuals

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../EnvironmentVisualization.cc               |   6 +-
 .../EnvironmentVisualization.hh               |   4 +-
 .../environment_preload/EnvironmentPreload.cc |  33 ++-
 .../environment_preload/VisualizationTool.hh  | 265 ++++++++++++++++++
 4 files changed, 300 insertions(+), 8 deletions(-)
 create mode 100644 src/systems/environment_preload/VisualizationTool.hh

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 3ed5f6f9cea..3da97f5ef17 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -51,9 +51,9 @@ namespace sim
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
 /// \brief Private data class for EnvironmentVisualization
-class EnvironmentVisualizationPrivate
+class EnvironmentVisualizationTool
 {
-  public: EnvironmentVisualizationPrivate()
+  public: EnvironmentVisualizationTool()
   {
     this->pcPub =
       this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
@@ -253,7 +253,7 @@ class EnvironmentVisualizationPrivate
 
 /////////////////////////////////////////////////
 EnvironmentVisualization::EnvironmentVisualization()
-  : GuiSystem(), dataPtr(new EnvironmentVisualizationPrivate)
+  : GuiSystem(), dataPtr(new EnvironmentVisualizationTool)
 {
   gui::App()->Engine()->rootContext()->setContextProperty(
       "EnvironmentVisualization", this);
diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
index 91cd420eb97..072a92e0f20 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
@@ -30,7 +30,7 @@ namespace sim
 // Inline bracket to help doxygen filtering.
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
-  class EnvironmentVisualizationPrivate;
+  class EnvironmentVisualizationTool;
 
   /// \class EnvironmentVisualization EnvironmentVisualization.hh
   ///     gz/sim/systems/EnvironmentVisualization.hh
@@ -66,7 +66,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
 
     /// \internal
     /// \brief Pointer to private data
-    private: std::unique_ptr<EnvironmentVisualizationPrivate> dataPtr;
+    private: std::unique_ptr<EnvironmentVisualizationTool> dataPtr;
 
     public: unsigned int xSamples{10};
 
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 0c449b966e2..e75125852eb 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -15,6 +15,7 @@
  *
  */
 #include "EnvironmentPreload.hh"
+#include "VisualizationTool.hh"
 
 #include <array>
 #include <string>
@@ -56,6 +57,15 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: std::atomic<bool> needsReload{false};
 
+  public: std::unique_ptr<EnvironmentVisualizationTool> visualizationPtr;
+
+  public: std::atomic<bool> visualize{false};
+
+  public: std::atomic<math::Vector3d> samples;
+
+  public: EnvironmentPreloadPrivate() :
+    visualizationPtr(new EnvironmentVisualizationTool) {};
+
   public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg)
   {
     std::lock_guard<std::mutex> lock(this->mtx);
@@ -63,6 +73,12 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     this->needsReload = true;
   }
 
+  public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
+  {
+    this->visualize = true;
+    this->samples = msgs::Convert(_resChanged);
+  }
+
   public: void ReadSdf(EntityComponentManager &_ecm)
   {
     if (!this->sdf->HasElement("data"))
@@ -243,7 +259,7 @@ void EnvironmentPreload::Configure(
 
 //////////////////////////////////////////////////
 void EnvironmentPreload::PreUpdate(
-  const gz::sim::UpdateInfo &,
+  const gz::sim::UpdateInfo &_info,
   gz::sim::EntityComponentManager &_ecm)
 {
   if (!std::exchange(this->dataPtr->loaded, true))
@@ -254,15 +270,26 @@ void EnvironmentPreload::PreUpdate(
     this->dataPtr->node.Subscribe(
       common::joinPaths(scopedName(world, _ecm),"environment"),
       &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get());
-      msgs::DataLoadPathOptions loadOptions;
+    this->dataPtr->node.Subscribe(
+      common::joinPaths(scopedName(world, _ecm),"environment/visualize/res"),
+      &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get());
+
+    this->dataPtr->visualizationPtr->resample = true;
 
     this->dataPtr->ReadSdf(_ecm);
   }
 
-  if (this->dataPtr->needsReload.load())
+  if (this->dataPtr->needsReload)
   {
     this->dataPtr->LoadEnvironment(_ecm);
   }
+
+  if (this->dataPtr->visualize)
+  {
+    auto samples = this->dataPtr->samples.load();
+    this->dataPtr->visualizationPtr->Step(_info, _ecm,
+      samples.X(), samples.Y(), samples.Z());
+  }
 }
 
 // Register this plugin
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
new file mode 100644
index 00000000000..b6dc33946b3
--- /dev/null
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -0,0 +1,265 @@
+/*
+ * 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 <atomic>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <gz/common/CSVStreams.hh>
+#include <gz/common/DataFrame.hh>
+
+#include <gz/transport/Node.hh>
+
+#include <gz/msgs/float_v.pb.h>
+#include <gz/msgs/pointcloud_packed.pb.h>
+#include <gz/msgs/PointCloudPackedUtils.hh>
+#include <gz/msgs/Utility.hh>
+
+#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
+{
+class EnvironmentVisualizationTool
+{
+  public: EnvironmentVisualizationTool()
+  {
+    this->pcPub =
+      this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
+  }
+  /// \brief To synchronize member access.
+  public: std::mutex mutex;
+
+  /// \brief first load we need to scan for existing data sensor
+  public: bool first {true};
+
+  public: std::atomic<bool> resample{true};
+
+  /////////////////////////////////////////////////
+  public: void CreatePointCloudTopics(
+    std::shared_ptr<components::EnvironmentalData> data) {
+    this->pubs.clear();
+    this->sessions.clear();
+    for (auto key : data->frame.Keys())
+    {
+      this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
+      gz::msgs::Float_V msg;
+      this->floatFields.emplace(key, msg);
+      this->sessions.emplace(key, data->frame[key].CreateSession());
+    }
+  }
+
+  /////////////////////////////////////////////////
+  public: void Step(
+    const UpdateInfo &_info,
+    const EntityComponentManager& _ecm,
+    double xSamples, double ySamples, double zSamples)
+  {
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> dt(now - this->lastTick);
+
+    std::shared_ptr<components::EnvironmentalData> data;
+
+    bool proceed{false};
+
+    _ecm.Each<components::Environment>([&](
+      const Entity &_e,
+      const components::Environment *_env
+    ) -> bool {
+      if (_env)
+      {
+        data = _env->Data();
+        proceed = true;
+      }
+    });
+
+    if (!proceed)
+      return;
+
+    if (this->resample)
+    {
+      this->CreatePointCloudTopics(data);
+      this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
+      this->resample = false;
+      this->lastTick = now;
+    }
+
+    for (auto &it : this->sessions)
+    {
+      auto res =
+        data->frame[it.first].StepTo(it.second,
+          std::chrono::duration<double>(_info.simTime).count());
+      if (res.has_value())
+      {
+        it.second = res.value();
+      }
+    }
+
+    // Publish at 2 hz for now. In future make reconfigureable.
+    if (dt.count() > 0.5)
+    {
+      this->Visualize(data, xSamples, ySamples, zSamples);
+      this->Publish();
+      lastTick = now;
+    }
+  }
+
+  /////////////////////////////////////////////////
+  public: void Visualize(
+    std::shared_ptr<components::EnvironmentalData> data,
+    double xSamples, double ySamples, double zSamples) {
+
+    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;
+      std::size_t idx = 0;
+      for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++)
+      {
+        auto x = lower_bound.X() + x_steps * dx;
+        for (std::size_t y_steps = 0; y_steps < ceil(ySamples); y_steps++)
+        {
+          auto y = lower_bound.Y() + y_steps * dy;
+          for (std::size_t z_steps = 0; z_steps < ceil(zSamples); z_steps++)
+          {
+            auto z = lower_bound.Z() + z_steps * dz;
+            auto res = frame.LookUp(session, math::Vector3d(x, y, z));
+
+            if (res.has_value())
+            {
+              this->floatFields[key].mutable_data()->Set(idx,
+                static_cast<float>(res.value()));
+            }
+            else
+            {
+              this->floatFields[key].mutable_data()->Set(idx, std::nanf(""));
+            }
+            idx++;
+          }
+        }
+      }
+    }
+  }
+
+  /////////////////////////////////////////////////
+  public: void Publish()
+  {
+    pcPub.Publish(this->pcMsg);
+    for(auto &[key, pub] : this->pubs)
+    {
+      pub.Publish(this->floatFields[key]);
+    }
+  }
+
+  /////////////////////////////////////////////////
+  public: void ResizeCloud(
+    std::shared_ptr<components::EnvironmentalData> data,
+    const EntityComponentManager& _ecm,
+    unsigned int xSamples, unsigned int ySamples, unsigned int zSamples)
+  {
+    assert (pubs.size() > 0);
+
+    // Assume all data have same point cloud.
+    gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
+        {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}});
+    auto numberOfPoints = xSamples * ySamples * zSamples;
+    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);
+
+    auto step = upper_bound - lower_bound;
+    auto dx = step.X() / xSamples;
+    auto dy = step.Y() / ySamples;
+    auto dz = step.Z() / zSamples;
+
+    // Populate point cloud
+    gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
+    gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
+    gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");
+
+    for (std::size_t x_steps = 0; x_steps < xSamples; x_steps++)
+    {
+      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;
+        for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++)
+        {
+          auto z = lower_bound.Z() + z_steps * dz;
+          auto coords = getGridFieldCoordinates(
+            _ecm, math::Vector3d{x, y, z},
+            data);
+
+          if (!coords.has_value())
+          {
+            continue;
+          }
+
+          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(""));
+    }
+  }
+
+  public: transport::Node::Publisher pcPub;
+  public: std::unordered_map<std::string, transport::Node::Publisher> pubs;
+  public: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
+  public: transport::Node node;
+  public: gz::msgs::PointCloudPacked pcMsg;
+  public: std::unordered_map<std::string,
+    gz::math::InMemorySession<double, double>> sessions;
+  public:  std::chrono::time_point<std::chrono::steady_clock> lastTick;
+};
+}
+}
+}
+#endif

From f0d18666c2372374e19a15b40c62883ac80e57bc Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Thu, 16 Feb 2023 05:17:09 +0000
Subject: [PATCH 04/28] Actually send message for loading from ui to
 environment preload plugin.

Visuallization still goes :boom:

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_loader/EnvironmentLoader.cc   |  2 ++
 .../environment_preload/EnvironmentPreload.cc | 20 +++++++++++++++----
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index 6f00178167c..40ca1c4f9fa 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -163,6 +163,8 @@ void EnvironmentLoader::ScheduleLoad()
 
     data.set_coordinate_type(msgs::ConvertCoord(referenceFrame));
     data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]);
+
+    this->dataPtr->pub->Publish(data);
   }
 }
 
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index e75125852eb..435ea226dbd 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -63,6 +63,8 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: std::atomic<math::Vector3d> samples;
 
+  public: bool logError{true};
+
   public: EnvironmentPreloadPrivate() :
     visualizationPtr(new EnvironmentVisualizationTool) {};
 
@@ -71,6 +73,8 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::lock_guard<std::mutex> lock(this->mtx);
     this->dataDescription = _msg;
     this->needsReload = true;
+    this->logError = true;
+    gzdbg << "Loading file " << _msg.path() << "\n";
   }
 
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
@@ -210,8 +214,12 @@ class gz::sim::systems::EnvironmentPreloadPrivate
       std::ifstream dataFile(this->dataDescription.path());
       if (!dataFile.is_open())
       {
-        gzerr << "No environmental data file was found at " <<
-          this->dataDescription.path();
+        if(logError)
+        {
+          gzerr << "No environmental data file was found at " <<
+            this->dataDescription.path() << std::endl;
+          logError = false;
+        }
         return;
       }
 
@@ -230,8 +238,12 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     }
     catch (const std::invalid_argument &exc)
     {
-      gzerr << "Failed to load environment data" << std::endl
-            << exc.what() << std::endl;
+      if(logError)
+      {
+        gzerr << "Failed to load environment data" << std::endl
+              << exc.what() << std::endl;
+        logError = false;
+      }
     }
 
     needsReload = false;

From 9b684cf5e6a353993e060e20a5fa6b912aab6745 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 17 Feb 2023 04:14:18 +0000
Subject: [PATCH 05/28] Rewrite EnvironmentVisualization Widget to be simpler.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../EnvironmentVisualization.cc               | 231 ++----------------
 .../environment_preload/EnvironmentPreload.cc |   7 +-
 .../environment_preload/VisualizationTool.hh  |  34 +++
 3 files changed, 64 insertions(+), 208 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 3da97f5ef17..962e2987c06 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -36,9 +36,7 @@
 
 #include <gz/transport/Node.hh>
 
-#include <gz/msgs/float_v.pb.h>
-#include <gz/msgs/pointcloud_packed.pb.h>
-#include <gz/msgs/PointCloudPackedUtils.hh>
+#include <gz/msgs/vector3d.pb.h>
 #include <gz/msgs/Utility.hh>
 
 using namespace gz;
@@ -53,199 +51,38 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
 /// \brief Private data class for EnvironmentVisualization
 class EnvironmentVisualizationTool
 {
-  public: EnvironmentVisualizationTool()
-  {
-    this->pcPub =
-      this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
-  }
-  /// \brief To synchronize member access.
-  public: std::mutex mutex;
-
-  /// \brief first load we need to scan for existing data sensor
-  public: bool first {true};
-
-  public: std::atomic<bool> resample{true};
-
-  /////////////////////////////////////////////////
-  public: void CreatePointCloudTopics(
-    std::shared_ptr<components::EnvironmentalData> data) {
-    this->pubs.clear();
-    this->sessions.clear();
-    for (auto key : data->frame.Keys())
-    {
-      this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
-      gz::msgs::Float_V msg;
-      this->floatFields.emplace(key, msg);
-      this->sessions.emplace(key, data->frame[key].CreateSession());
-    }
-  }
 
   /////////////////////////////////////////////////
-  public: void Step(
-    const UpdateInfo &_info,
-    std::shared_ptr<components::EnvironmentalData> &data,
-    const EntityComponentManager& _ecm,
-    double xSamples, double ySamples, double zSamples)
+  public: void Initiallize(
+    const EntityComponentManager &_ecm)
   {
-    auto now = std::chrono::steady_clock::now();
-    std::chrono::duration<double> dt(now - this->lastTick);
-
-    if (this->resample)
-    {
-      this->CreatePointCloudTopics(data);
-      this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
-      this->resample = false;
-      this->lastTick = now;
-    }
-
-    for (auto &it : this->sessions)
-    {
-      auto res =
-        data->frame[it.first].StepTo(it.second,
-          std::chrono::duration<double>(_info.simTime).count());
-      if (res.has_value())
-      {
-        it.second = res.value();
-      }
-    }
-
-    // Publish at 2 hz for now. In future make reconfigureable.
-    if (dt.count() > 0.5)
-    {
-      this->Visualize(data, xSamples, ySamples, zSamples);
-      this->Publish();
-      lastTick = now;
-    }
+    auto world = worldEntity(_ecm);
+    auto topic = 
+      common::joinPaths(
+        scopedName(world, _ecm),"environment/visualize/res");
+    std::lock_guard<std::mutex> lock(this->mutex);
+    this->pcPub = node.Advertise<gz::msgs::Vector3d>(topic);
   }
 
   /////////////////////////////////////////////////
   public: void Visualize(
-    std::shared_ptr<components::EnvironmentalData> data,
-    double xSamples, double ySamples, double zSamples) {
-
-    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;
-      std::size_t idx = 0;
-      for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++)
-      {
-        auto x = lower_bound.X() + x_steps * dx;
-        for (std::size_t y_steps = 0; y_steps < ceil(ySamples); y_steps++)
-        {
-          auto y = lower_bound.Y() + y_steps * dy;
-          for (std::size_t z_steps = 0; z_steps < ceil(zSamples); z_steps++)
-          {
-            auto z = lower_bound.Z() + z_steps * dz;
-            auto res = frame.LookUp(session, math::Vector3d(x, y, z));
-
-            if (res.has_value())
-            {
-              this->floatFields[key].mutable_data()->Set(idx,
-                static_cast<float>(res.value()));
-            }
-            else
-            {
-              this->floatFields[key].mutable_data()->Set(idx, std::nanf(""));
-            }
-            idx++;
-          }
-        }
-      }
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void Publish()
+    double xSamples, double ySamples, double zSamples)
   {
-    pcPub.Publish(this->pcMsg);
-    for(auto &[key, pub] : this->pubs)
-    {
-      pub.Publish(this->floatFields[key]);
-    }
+    std::lock_guard<std::mutex> lock(this->mutex);
+    this->vec = msgs::Convert(math::Vector3d(xSamples, ySamples, zSamples));
+    this->pcPub.Publish(vec);
   }
 
-  /////////////////////////////////////////////////
-  public: void ResizeCloud(
-    std::shared_ptr<components::EnvironmentalData> data,
-    const EntityComponentManager& _ecm,
-    unsigned int xSamples, unsigned int ySamples, unsigned int zSamples)
-  {
-    assert (pubs.size() > 0);
-
-    // Assume all data have same point cloud.
-    gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
-        {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}});
-    auto numberOfPoints = xSamples * ySamples * zSamples;
-    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);
-
-    auto step = upper_bound - lower_bound;
-    auto dx = step.X() / xSamples;
-    auto dy = step.Y() / ySamples;
-    auto dz = step.Z() / zSamples;
-
-    // Populate point cloud
-    gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
-    gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
-    gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");
-
-    for (std::size_t x_steps = 0; x_steps < xSamples; x_steps++)
-    {
-      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;
-        for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++)
-        {
-          auto z = lower_bound.Z() + z_steps * dz;
-          auto coords = getGridFieldCoordinates(
-            _ecm, math::Vector3d{x, y, z},
-            data);
+  public: gz::msgs::Vector3d vec;
+  public: transport::Node::Publisher pcPub;
 
-          if (!coords.has_value())
-          {
-            continue;
-          }
+  public: transport::Node node;
 
-          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(""));
-    }
-  }
+  /// \brief To synchronize member access.
+  public: std::mutex mutex;
 
-  public: transport::Node::Publisher pcPub;
-  public: std::unordered_map<std::string, transport::Node::Publisher> pubs;
-  public: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
-  public: transport::Node node;
-  public: gz::msgs::PointCloudPacked pcMsg;
-  public: std::unordered_map<std::string,
-    gz::math::InMemorySession<double, double>> sessions;
-  public:  std::chrono::time_point<std::chrono::steady_clock> lastTick;
+  /// \brief first load we need to scan for existing data sensor
+  public: bool first {true};
 };
 }
 }
@@ -277,35 +114,19 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *)
 void EnvironmentVisualization::Update(const UpdateInfo &_info,
                                EntityComponentManager &_ecm)
 {
-  _ecm.EachNew<components::Environment>(
-    [this](
-      const Entity &/*_entity*/,
-      const components::Environment* /*environment*/
-    ) {
-      this->dataPtr->resample = true;
-      return true;
-    }
-  );
-
-  auto environData =
-    _ecm.Component<components::Environment>(
-      worldEntity(_ecm));
-
-  if (environData == nullptr)
+  if (this->dataPtr->first)
   {
-    return;
+    this->dataPtr->Initiallize(_ecm);
+    this->dataPtr->first = false;
   }
-
-  this->dataPtr->Step(
-    _info, environData->Data(), _ecm,
-    this->xSamples, this->ySamples, this->zSamples
-  );
 }
 
 /////////////////////////////////////////////////
 void EnvironmentVisualization::ResamplePointcloud()
 {
-  this->dataPtr->resample = true;
+  this->dataPtr->Visualize(
+    this->xSamples, this->ySamples, this->zSamples
+  );
 }
 
 
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 435ea226dbd..c8375c4dbe1 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -59,9 +59,9 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: std::unique_ptr<EnvironmentVisualizationTool> visualizationPtr;
 
-  public: std::atomic<bool> visualize{false};
+  public: bool visualize{false};
 
-  public: std::atomic<math::Vector3d> samples;
+  public: math::Vector3d samples;
 
   public: bool logError{true};
 
@@ -79,6 +79,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
   {
+    std::lock_guard<std::mutex> lock(this->mtx);
     this->visualize = true;
     this->samples = msgs::Convert(_resChanged);
   }
@@ -298,7 +299,7 @@ void EnvironmentPreload::PreUpdate(
 
   if (this->dataPtr->visualize)
   {
-    auto samples = this->dataPtr->samples.load();
+    auto samples = this->dataPtr->samples;
     this->dataPtr->visualizationPtr->Step(_info, _ecm,
       samples.X(), samples.Y(), samples.Z());
   }
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index b6dc33946b3..1f3bfac3bd6 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -64,13 +64,21 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
+
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
     this->pubs.clear();
     this->sessions.clear();
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     for (auto key : data->frame.Keys())
     {
+          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
       gz::msgs::Float_V msg;
       this->floatFields.emplace(key, msg);
+          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->sessions.emplace(key, data->frame[key].CreateSession());
     }
   }
@@ -81,11 +89,17 @@ class EnvironmentVisualizationTool
     const EntityComponentManager& _ecm,
     double xSamples, double ySamples, double zSamples)
   {
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     auto now = std::chrono::steady_clock::now();
     std::chrono::duration<double> dt(now - this->lastTick);
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     std::shared_ptr<components::EnvironmentalData> data;
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     bool proceed{false};
 
     _ecm.Each<components::Environment>([&](
@@ -94,22 +108,40 @@ class EnvironmentVisualizationTool
     ) -> bool {
       if (_env)
       {
+    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
         data = _env->Data();
         proceed = true;
+    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       }
     });
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     if (!proceed)
       return;
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     if (this->resample)
     {
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->CreatePointCloudTopics(data);
+
+
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
       this->resample = false;
       this->lastTick = now;
+        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+  
     }
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     for (auto &it : this->sessions)
     {
       auto res =
@@ -120,6 +152,8 @@ class EnvironmentVisualizationTool
         it.second = res.value();
       }
     }
+        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
 
     // Publish at 2 hz for now. In future make reconfigureable.
     if (dt.count() > 0.5)

From dd24227dad96735d96c47425b9fccaefbcfa86fe Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 17 Feb 2023 06:39:28 +0000
Subject: [PATCH 06/28] fix crashes.

Vis still not working

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/EnvironmentPreload.cc |  6 ++-
 .../environment_preload/VisualizationTool.hh  | 47 ++-----------------
 2 files changed, 8 insertions(+), 45 deletions(-)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index c8375c4dbe1..a4f516fc5ca 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -65,6 +65,8 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: bool logError{true};
 
+  public: std::shared_ptr<components::EnvironmentalData> envData;
+
   public: EnvironmentPreloadPrivate() :
     visualizationPtr(new EnvironmentVisualizationTool) {};
 
@@ -232,6 +234,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
               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)};
@@ -299,8 +302,9 @@ void EnvironmentPreload::PreUpdate(
 
   if (this->dataPtr->visualize)
   {
+    std::lock_guard<std::mutex> lock(this->dataPtr->mtx);
     auto samples = this->dataPtr->samples;
-    this->dataPtr->visualizationPtr->Step(_info, _ecm,
+    this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData,
       samples.X(), samples.Y(), samples.Z());
   }
 }
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 1f3bfac3bd6..13e944eac2c 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -64,21 +64,13 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
-
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
     this->pubs.clear();
     this->sessions.clear();
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
     for (auto key : data->frame.Keys())
     {
-          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
       this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
       gz::msgs::Float_V msg;
       this->floatFields.emplace(key, msg);
-          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
       this->sessions.emplace(key, data->frame[key].CreateSession());
     }
   }
@@ -87,61 +79,28 @@ class EnvironmentVisualizationTool
   public: void Step(
     const UpdateInfo &_info,
     const EntityComponentManager& _ecm,
+    std::shared_ptr<components::EnvironmentalData> data,
     double xSamples, double ySamples, double zSamples)
   {
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
 
     auto now = std::chrono::steady_clock::now();
     std::chrono::duration<double> dt(now - this->lastTick);
-
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
-    std::shared_ptr<components::EnvironmentalData> data;
-
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+    ;
 
     bool proceed{false};
 
-    _ecm.Each<components::Environment>([&](
-      const Entity &_e,
-      const components::Environment *_env
-    ) -> bool {
-      if (_env)
-      {
-    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
-        data = _env->Data();
-        proceed = true;
-    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
-      }
-    });
-
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
     if (!proceed)
       return;
 
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
 
     if (this->resample)
     {
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
       this->CreatePointCloudTopics(data);
-
-
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
       this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
       this->resample = false;
-      this->lastTick = now;
-        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-  
+      this->lastTick = now;  
     }
 
-    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
     for (auto &it : this->sessions)
     {
       auto res =

From 712ebe0c5f9bc7c3aaed1b6950e7eb13622e5b67 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 17 Feb 2023 08:16:31 +0000
Subject: [PATCH 07/28] Get a different :boom:

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/VisualizationTool.hh       | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 13e944eac2c..3928cc1c057 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -64,14 +64,19 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
+    std::cout << __FILE__ << __LINE__ <<std::endl;
     this->pubs.clear();
     this->sessions.clear();
+    std::cout << __FILE__ << __LINE__ <<std::endl;
+
     for (auto key : data->frame.Keys())
     {
       this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
       gz::msgs::Float_V msg;
       this->floatFields.emplace(key, msg);
       this->sessions.emplace(key, data->frame[key].CreateSession());
+      std::cout << __FILE__ << __LINE__ <<std::endl;
+
     }
   }
 
@@ -85,13 +90,6 @@ class EnvironmentVisualizationTool
 
     auto now = std::chrono::steady_clock::now();
     std::chrono::duration<double> dt(now - this->lastTick);
-    ;
-
-    bool proceed{false};
-
-    if (!proceed)
-      return;
-
 
     if (this->resample)
     {
@@ -111,8 +109,6 @@ class EnvironmentVisualizationTool
         it.second = res.value();
       }
     }
-        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
-
 
     // Publish at 2 hz for now. In future make reconfigureable.
     if (dt.count() > 0.5)

From d722ad81e4a7598f1be1d300b187246670672ba3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 17 Feb 2023 14:23:47 +0000
Subject: [PATCH 08/28] Works some times.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_visualization/EnvironmentVisualization.cc    | 1 +
 src/systems/environment_preload/EnvironmentPreload.cc        | 2 ++
 src/systems/environment_preload/VisualizationTool.hh         | 5 +----
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 962e2987c06..c448d409e45 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -118,6 +118,7 @@ void EnvironmentVisualization::Update(const UpdateInfo &_info,
   {
     this->dataPtr->Initiallize(_ecm);
     this->dataPtr->first = false;
+    //this->ResamplePointcloud();
   }
 }
 
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index a4f516fc5ca..d94197a7844 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -84,6 +84,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::lock_guard<std::mutex> lock(this->mtx);
     this->visualize = true;
     this->samples = msgs::Convert(_resChanged);
+    this->visualizationPtr->resample = true;
   }
 
   public: void ReadSdf(EntityComponentManager &_ecm)
@@ -239,6 +240,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;
     }
     catch (const std::invalid_argument &exc)
     {
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 3928cc1c057..067a79c9996 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -64,10 +64,9 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
-    std::cout << __FILE__ << __LINE__ <<std::endl;
+    
     this->pubs.clear();
     this->sessions.clear();
-    std::cout << __FILE__ << __LINE__ <<std::endl;
 
     for (auto key : data->frame.Keys())
     {
@@ -75,8 +74,6 @@ class EnvironmentVisualizationTool
       gz::msgs::Float_V msg;
       this->floatFields.emplace(key, msg);
       this->sessions.emplace(key, data->frame[key].CreateSession());
-      std::cout << __FILE__ << __LINE__ <<std::endl;
-
     }
   }
 

From 87586d786899b52cf04ecd964da2cc1a5f5b36e6 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 20 Feb 2023 18:25:39 +0000
Subject: [PATCH 09/28] Fixed synchronization issues.

Now left with one more crash that needs debugging when "play" is hit.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../EnvironmentVisualization.cc                        |  6 +++++-
 .../EnvironmentVisualization.hh                        |  2 ++
 src/systems/environment_preload/EnvironmentPreload.cc  | 10 ++++++++--
 src/systems/environment_preload/VisualizationTool.hh   |  2 ++
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index c448d409e45..46feaef9c85 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -94,6 +94,10 @@ EnvironmentVisualization::EnvironmentVisualization()
 {
   gui::App()->Engine()->rootContext()->setContextProperty(
       "EnvironmentVisualization", this);
+  this->qtimer = new QTimer(this);
+  connect(qtimer, &QTimer::timeout,
+    this, &EnvironmentVisualization::ResamplePointcloud);
+  this->qtimer->start(1000);
 }
 
 /////////////////////////////////////////////////
@@ -118,7 +122,7 @@ void EnvironmentVisualization::Update(const UpdateInfo &_info,
   {
     this->dataPtr->Initiallize(_ecm);
     this->dataPtr->first = false;
-    //this->ResamplePointcloud();
+    this->ResamplePointcloud();
   }
 }
 
diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
index 072a92e0f20..e5a3eb43970 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh
@@ -73,6 +73,8 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
     public: unsigned int ySamples{10};
 
     public: unsigned int zSamples{10};
+
+    private: QTimer* qtimer;
   };
 }
 }
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index d94197a7844..8aebb51745e 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -63,6 +63,8 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: math::Vector3d samples;
 
+  public: bool fileLoaded{false};
+
   public: bool logError{true};
 
   public: std::shared_ptr<components::EnvironmentalData> envData;
@@ -82,9 +84,12 @@ class gz::sim::systems::EnvironmentPreloadPrivate
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
   {
     std::lock_guard<std::mutex> lock(this->mtx);
-    this->visualize = true;
     this->samples = msgs::Convert(_resChanged);
-    this->visualizationPtr->resample = true;
+    if (this->fileLoaded)
+    {
+      this->visualize = true;
+      this->visualizationPtr->resample = true;
+    }
   }
 
   public: void ReadSdf(EntityComponentManager &_ecm)
@@ -241,6 +246,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
       auto component = ComponentT{std::move(data)};
       _ecm.CreateComponent(worldEntity(_ecm), std::move(component));
       this->visualizationPtr->resample = true;
+      this->fileLoaded = true;
     }
     catch (const std::invalid_argument &exc)
     {
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 067a79c9996..32457c5a7d5 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -61,6 +61,8 @@ class EnvironmentVisualizationTool
 
   public: std::atomic<bool> resample{true};
 
+  public: bool finishedTime{false};
+
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {

From 77f54723265bd28e9d0db52278dd5513cddb8fd6 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Tue, 21 Feb 2023 11:11:45 +0000
Subject: [PATCH 10/28] No more :boom:s :tada:

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/EnvironmentPreload.cc | 19 +++++++++++++++----
 .../environment_preload/VisualizationTool.hh  | 13 ++++++++++++-
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 8aebb51745e..aa109c260c4 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -78,18 +78,29 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     this->dataDescription = _msg;
     this->needsReload = true;
     this->logError = true;
+    this->visualizationPtr->FileReloaded();
     gzdbg << "Loading file " << _msg.path() << "\n";
   }
 
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
   {
     std::lock_guard<std::mutex> lock(this->mtx);
-    this->samples = msgs::Convert(_resChanged);
-    if (this->fileLoaded)
+    if (!this->fileLoaded)
     {
-      this->visualize = true;
-      this->visualizationPtr->resample = true;
+      // Only visualize if a file exists
+      return;
+    }
+    auto converted = msgs::Convert(_resChanged);
+    if (this->samples  == converted)
+    {
+      // If the sample has not changed return.
+      // This is because resampling is expensive.
+      return;
     }
+    this->samples = converted;
+    this->visualize = true;
+    this->visualizationPtr->resample = true;
+  
   }
 
   public: void ReadSdf(EntityComponentManager &_ecm)
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 32457c5a7d5..00f8934a1f8 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -79,6 +79,12 @@ class EnvironmentVisualizationTool
     }
   }
 
+  /////////////////////////////////////////////////
+  public: void FileReloaded()
+  {
+    this->finishedTime = false;
+  }
+
   /////////////////////////////////////////////////
   public: void Step(
     const UpdateInfo &_info,
@@ -107,10 +113,15 @@ class EnvironmentVisualizationTool
       {
         it.second = res.value();
       }
+      else 
+      {
+        this->finishedTime = true;
+        return;
+      }
     }
 
     // Publish at 2 hz for now. In future make reconfigureable.
-    if (dt.count() > 0.5)
+    if (dt.count() > 0.5 && !this->finishedTime)
     {
       this->Visualize(data, xSamples, ySamples, zSamples);
       this->Publish();

From 882edbe5a0980d327a4676f8a724514af8dd23a6 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Wed, 22 Feb 2023 00:32:17 +0000
Subject: [PATCH 11/28] style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_visualization/EnvironmentVisualization.cc   | 2 +-
 src/systems/environment_preload/EnvironmentPreload.cc       | 1 -
 src/systems/environment_preload/VisualizationTool.hh        | 6 +++---
 3 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 46feaef9c85..a23303e1bab 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -57,7 +57,7 @@ class EnvironmentVisualizationTool
     const EntityComponentManager &_ecm)
   {
     auto world = worldEntity(_ecm);
-    auto topic = 
+    auto topic =
       common::joinPaths(
         scopedName(world, _ecm),"environment/visualize/res");
     std::lock_guard<std::mutex> lock(this->mutex);
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index aa109c260c4..36f8ff6bdd9 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -100,7 +100,6 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     this->samples = converted;
     this->visualize = true;
     this->visualizationPtr->resample = true;
-  
   }
 
   public: void ReadSdf(EntityComponentManager &_ecm)
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 00f8934a1f8..3fa1b79da0e 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -66,7 +66,7 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
-    
+
     this->pubs.clear();
     this->sessions.clear();
 
@@ -101,7 +101,7 @@ class EnvironmentVisualizationTool
       this->CreatePointCloudTopics(data);
       this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
       this->resample = false;
-      this->lastTick = now;  
+      this->lastTick = now;
     }
 
     for (auto &it : this->sessions)
@@ -113,7 +113,7 @@ class EnvironmentVisualizationTool
       {
         it.second = res.value();
       }
-      else 
+      else
       {
         this->finishedTime = true;
         return;

From ea93a1bb7d435a77109469e54d058dfae80e629d Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Wed, 22 Feb 2023 09:16:56 +0000
Subject: [PATCH 12/28] Sprinkled with healthy dose of Doxygen

Also refactored the visualization tool out.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../EnvironmentVisualization.cc               |   4 +
 .../environment_preload/CMakeLists.txt        |   1 +
 .../environment_preload/EnvironmentPreload.cc |  30 +-
 .../environment_preload/VisualizationTool.cc  | 211 ++++++++++++++
 .../environment_preload/VisualizationTool.hh  | 273 +++++-------------
 5 files changed, 314 insertions(+), 205 deletions(-)
 create mode 100644 src/systems/environment_preload/VisualizationTool.cc

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index a23303e1bab..24afad83fe5 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -73,9 +73,13 @@ class EnvironmentVisualizationTool
     this->pcPub.Publish(vec);
   }
 
+  /// \brief The sample resolution
   public: gz::msgs::Vector3d vec;
+  
+  /// \brief Publisher to publish sample resolution
   public: transport::Node::Publisher pcPub;
 
+  /// \brief Gz transport node
   public: transport::Node node;
 
   /// \brief To synchronize member access.
diff --git a/src/systems/environment_preload/CMakeLists.txt b/src/systems/environment_preload/CMakeLists.txt
index 178ec5ec329..ed6a4b8efe5 100644
--- a/src/systems/environment_preload/CMakeLists.txt
+++ b/src/systems/environment_preload/CMakeLists.txt
@@ -1,6 +1,7 @@
 gz_add_system(environment-preload
   SOURCES
     EnvironmentPreload.cc
+    VisualizationTool.cc
   PUBLIC_LINK_LIBS
     gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
     gz-common${GZ_COMMON_VER}::io
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 36f8ff6bdd9..a746b76b0ed 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -45,43 +45,58 @@ 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<const sdf::Element> 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<bool> needsReload{false};
 
+  /// \brief Visualization Helper
   public: std::unique_ptr<EnvironmentVisualizationTool> visualizationPtr;
 
+  /// \brief Are visualizations enabled
   public: bool visualize{false};
 
+  /// \brief Sample resolutions
   public: math::Vector3d samples;
 
+  /// \brief Is the file loaded
   public: bool fileLoaded{false};
 
-  public: bool logError{true};
+  /// \brief File loading error logger
+  public: bool logFileLoadError{true};
 
+  /// \brief Reference to data
   public: std::shared_ptr<components::EnvironmentalData> envData;
 
+  //////////////////////////////////////////////////
   public: EnvironmentPreloadPrivate() :
     visualizationPtr(new EnvironmentVisualizationTool) {};
 
+  //////////////////////////////////////////////////
   public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg)
   {
     std::lock_guard<std::mutex> lock(this->mtx);
     this->dataDescription = _msg;
     this->needsReload = true;
-    this->logError = true;
+    this->logFileLoadError = true;
     this->visualizationPtr->FileReloaded();
     gzdbg << "Loading file " << _msg.path() << "\n";
   }
 
+  //////////////////////////////////////////////////
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
   {
     std::lock_guard<std::mutex> lock(this->mtx);
@@ -102,6 +117,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     this->visualizationPtr->resample = true;
   }
 
+  //////////////////////////////////////////////////
   public: void ReadSdf(EntityComponentManager &_ecm)
   {
     if (!this->sdf->HasElement("data"))
@@ -202,6 +218,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     needsReload = true;
   }
 
+  //////////////////////////////////////////////////
   public: components::EnvironmentalData::ReferenceUnits ConvertUnits(
     const Units &_unit)
   {
@@ -216,6 +233,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     }
   }
 
+  //////////////////////////////////////////////////
   public: void LoadEnvironment(EntityComponentManager &_ecm)
   {
     try
@@ -233,11 +251,11 @@ class gz::sim::systems::EnvironmentPreloadPrivate
       std::ifstream dataFile(this->dataDescription.path());
       if (!dataFile.is_open())
       {
-        if(logError)
+        if(logFileLoadError)
         {
           gzerr << "No environmental data file was found at " <<
             this->dataDescription.path() << std::endl;
-          logError = false;
+          logFileLoadError = false;
         }
         return;
       }
@@ -260,11 +278,11 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     }
     catch (const std::invalid_argument &exc)
     {
-      if(logError)
+      if(logFileLoadError)
       {
         gzerr << "Failed to load environment data" << std::endl
               << exc.what() << std::endl;
-        logError = false;
+        logFileLoadError = false;
       }
     }
 
diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
new file mode 100644
index 00000000000..e906e75d615
--- /dev/null
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -0,0 +1,211 @@
+/*
+ * 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<gz::msgs::PointCloudPacked>("/point_cloud");
+}
+
+/////////////////////////////////////////////////
+void EnvironmentVisualizationTool::CreatePointCloudTopics(
+    const std::shared_ptr<components::EnvironmentalData> _data) {
+  this->pubs.clear();
+  this->sessions.clear();
+
+  for (auto key : _data->frame.Keys())
+  {
+      this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
+      gz::msgs::Float_V msg;
+      this->floatFields.emplace(key, msg);
+      this->sessions.emplace(key, _data->frame[key].CreateSession());
+  }
+}
+
+/////////////////////////////////////////////////
+void EnvironmentVisualizationTool::FileReloaded()
+{
+  this->finishedTime = false;
+}
+
+/////////////////////////////////////////////////
+void EnvironmentVisualizationTool::Step(
+    const UpdateInfo &_info,
+    const EntityComponentManager& _ecm,
+    const std::shared_ptr<components::EnvironmentalData> _data,
+    double _xSamples, double _ySamples, double _zSamples)
+{
+
+  auto now = std::chrono::steady_clock::now();
+  std::chrono::duration<double> dt(now - this->lastTick);
+
+  if (this->resample)
+  {
+    this->CreatePointCloudTopics(_data);
+    this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples);
+    this->resample = false;
+    this->lastTick = now;
+  }
+
+  for (auto &it : this->sessions)
+  {
+    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
+    {
+      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<components::EnvironmentalData> data,
+    double _xSamples, double _ySamples, double _zSamples) 
+{
+
+  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;
+    std::size_t idx = 0;
+    for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++)
+    {
+      auto x = lower_bound.X() + x_steps * dx;
+      for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++)
+      {
+        auto y = lower_bound.Y() + y_steps * dy;
+        for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++)
+        {
+          auto z = lower_bound.Z() + z_steps * dz;
+          auto res = frame.LookUp(session, math::Vector3d(x, y, z));
+
+          if (res.has_value())
+          {
+            this->floatFields[key].mutable_data()->Set(idx,
+              static_cast<float>(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<components::EnvironmentalData> _data,
+  const EntityComponentManager& _ecm,
+  unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples)
+{
+  assert (pubs.size() > 0);
+
+  // Assume all data have same point cloud.
+  gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
+      {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}});
+  auto numberOfPoints = _xSamples * _ySamples * _zSamples;
+  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);
+
+  auto step = upper_bound - lower_bound;
+  auto dx = step.X() / _xSamples;
+  auto dy = step.Y() / _ySamples;
+  auto dz = step.Z() / _zSamples;
+
+  // Populate point cloud
+  gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
+  gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
+  gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");
+
+  for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++)
+  {
+    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;
+      for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++)
+      {
+        auto z = lower_bound.Z() + z_steps * dz;
+        auto coords = getGridFieldCoordinates(
+          _ecm, math::Vector3d{x, y, z},
+          _data);
+
+        if (!coords.has_value())
+        {
+          continue;
+        }
+
+        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
index 3fa1b79da0e..3c85e5eee45 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -46,217 +46,92 @@ namespace sim
 {
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
+
+/// \brief This class helps 
 class EnvironmentVisualizationTool
 {
-  public: EnvironmentVisualizationTool()
-  {
-    this->pcPub =
-      this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
-  }
+  /// \brief Environment constructor
+  public: EnvironmentVisualizationTool();
+  
   /// \brief To synchronize member access.
-  public: std::mutex mutex;
+  private: std::mutex mutex;
 
-  /// \brief first load we need to scan for existing data sensor
-  public: bool first {true};
+  /// \brief First load we need to scan for existing data sensor
+  private: bool first {true};
 
+  /// \brief Enable resampling
   public: std::atomic<bool> resample{true};
 
-  public: bool finishedTime{false};
-
-  /////////////////////////////////////////////////
-  public: void CreatePointCloudTopics(
-    std::shared_ptr<components::EnvironmentalData> data) {
-
-    this->pubs.clear();
-    this->sessions.clear();
-
-    for (auto key : data->frame.Keys())
-    {
-      this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
-      gz::msgs::Float_V msg;
-      this->floatFields.emplace(key, msg);
-      this->sessions.emplace(key, data->frame[key].CreateSession());
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void FileReloaded()
-  {
-    this->finishedTime = false;
-  }
-
-  /////////////////////////////////////////////////
+  /// \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 visuallized
+  private: void CreatePointCloudTopics(
+    const std::shared_ptr<components::EnvironmentalData> _data);
+
+  /// \brief Invoke when new file is made available.
+  public: void FileReloaded();
+
+  /// \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 visuallized
+  /// \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,
-    std::shared_ptr<components::EnvironmentalData> data,
-    double xSamples, double ySamples, double zSamples)
-  {
-
-    auto now = std::chrono::steady_clock::now();
-    std::chrono::duration<double> dt(now - this->lastTick);
-
-    if (this->resample)
-    {
-      this->CreatePointCloudTopics(data);
-      this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
-      this->resample = false;
-      this->lastTick = now;
-    }
-
-    for (auto &it : this->sessions)
-    {
-      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
-      {
-        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;
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void Visualize(
-    std::shared_ptr<components::EnvironmentalData> data,
-    double xSamples, double ySamples, double zSamples) {
-
-    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;
-      std::size_t idx = 0;
-      for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++)
-      {
-        auto x = lower_bound.X() + x_steps * dx;
-        for (std::size_t y_steps = 0; y_steps < ceil(ySamples); y_steps++)
-        {
-          auto y = lower_bound.Y() + y_steps * dy;
-          for (std::size_t z_steps = 0; z_steps < ceil(zSamples); z_steps++)
-          {
-            auto z = lower_bound.Z() + z_steps * dz;
-            auto res = frame.LookUp(session, math::Vector3d(x, y, z));
-
-            if (res.has_value())
-            {
-              this->floatFields[key].mutable_data()->Set(idx,
-                static_cast<float>(res.value()));
-            }
-            else
-            {
-              this->floatFields[key].mutable_data()->Set(idx, std::nanf(""));
-            }
-            idx++;
-          }
-        }
-      }
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void Publish()
-  {
-    pcPub.Publish(this->pcMsg);
-    for(auto &[key, pub] : this->pubs)
-    {
-      pub.Publish(this->floatFields[key]);
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void ResizeCloud(
-    std::shared_ptr<components::EnvironmentalData> data,
+    const std::shared_ptr<components::EnvironmentalData> _data,
+    double _xSamples, double _ySamples, double _zSamples);
+
+  /// \brief Publishes a sample of the data
+  /// \param[in] _data The data to be visuallized
+  /// \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<components::EnvironmentalData> data,
+    double _xSamples, double _ySamples, double _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 visuallized
+  /// \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<components::EnvironmentalData> _data,
     const EntityComponentManager& _ecm,
-    unsigned int xSamples, unsigned int ySamples, unsigned int zSamples)
-  {
-    assert (pubs.size() > 0);
-
-    // Assume all data have same point cloud.
-    gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
-        {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}});
-    auto numberOfPoints = xSamples * ySamples * zSamples;
-    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);
-
-    auto step = upper_bound - lower_bound;
-    auto dx = step.X() / xSamples;
-    auto dy = step.Y() / ySamples;
-    auto dz = step.Z() / zSamples;
-
-    // Populate point cloud
-    gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
-    gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
-    gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");
-
-    for (std::size_t x_steps = 0; x_steps < xSamples; x_steps++)
-    {
-      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;
-        for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++)
-        {
-          auto z = lower_bound.Z() + z_steps * dz;
-          auto coords = getGridFieldCoordinates(
-            _ecm, math::Vector3d{x, y, z},
-            data);
-
-          if (!coords.has_value())
-          {
-            continue;
-          }
-
-          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(""));
-    }
-  }
-
-  public: transport::Node::Publisher pcPub;
-  public: std::unordered_map<std::string, transport::Node::Publisher> pubs;
-  public: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
-  public: transport::Node node;
-  public: gz::msgs::PointCloudPacked pcMsg;
-  public: std::unordered_map<std::string,
+    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<std::string, transport::Node::Publisher> pubs;
+  
+  /// \brief Floating point message buffers
+  private: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
+  
+  /// \brief GZ buffers
+  private: transport::Node node;
+
+  /// \brief Point cloud buffer
+  private: gz::msgs::PointCloudPacked pcMsg;
+  
+  /// \brief Session cursors
+  private: std::unordered_map<std::string,
     gz::math::InMemorySession<double, double>> sessions;
-  public:  std::chrono::time_point<std::chrono::steady_clock> lastTick;
+
+  /// \brief Duration from last update
+  private:  std::chrono::time_point<std::chrono::steady_clock> lastTick;
 };
 }
 }

From 4fd5c9eda82029a4f54a8bf4c65834e84a4b9bcb Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Thu, 23 Feb 2023 07:24:03 +0000
Subject: [PATCH 13/28] Style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../EnvironmentVisualization.cc                       |  2 +-
 src/systems/environment_preload/VisualizationTool.cc  | 10 +++++-----
 src/systems/environment_preload/VisualizationTool.hh  | 11 ++++++-----
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 24afad83fe5..a8cec773d2f 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -75,7 +75,7 @@ class EnvironmentVisualizationTool
 
   /// \brief The sample resolution
   public: gz::msgs::Vector3d vec;
-  
+
   /// \brief Publisher to publish sample resolution
   public: transport::Node::Publisher pcPub;
 
diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index e906e75d615..a444ed59920 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -32,10 +32,10 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics(
 
   for (auto key : _data->frame.Keys())
   {
-      this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
-      gz::msgs::Float_V msg;
-      this->floatFields.emplace(key, msg);
-      this->sessions.emplace(key, _data->frame[key].CreateSession());
+    this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
+    gz::msgs::Float_V msg;
+    this->floatFields.emplace(key, msg);
+    this->sessions.emplace(key, _data->frame[key].CreateSession());
   }
 }
 
@@ -92,7 +92,7 @@ void EnvironmentVisualizationTool::Step(
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::Visualize(
     const std::shared_ptr<components::EnvironmentalData> data,
-    double _xSamples, double _ySamples, double _zSamples) 
+    double _xSamples, double _ySamples, double _zSamples)
 {
 
   for (auto key : data->frame.Keys())
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 3c85e5eee45..5bd6a4eef66 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -47,12 +47,13 @@ namespace sim
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
 
-/// \brief This class helps 
+/// \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;
 
@@ -116,16 +117,16 @@ class EnvironmentVisualizationTool
 
   /// \brief Publishers for data
   private: std::unordered_map<std::string, transport::Node::Publisher> pubs;
-  
+
   /// \brief Floating point message buffers
   private: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
-  
+
   /// \brief GZ buffers
   private: transport::Node node;
 
   /// \brief Point cloud buffer
   private: gz::msgs::PointCloudPacked pcMsg;
-  
+
   /// \brief Session cursors
   private: std::unordered_map<std::string,
     gz::math::InMemorySession<double, double>> sessions;

From d7c34c330c43a27fcd9f4c2a63a2627651b6a196 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Thu, 23 Feb 2023 08:06:57 +0000
Subject: [PATCH 14/28] More style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 src/gui/plugins/environment_loader/EnvironmentLoader.cc    | 2 +-
 .../environment_visualization/EnvironmentVisualization.cc  | 2 +-
 src/systems/environment_preload/EnvironmentPreload.cc      | 7 ++++---
 src/systems/environment_preload/VisualizationTool.hh       | 1 +
 4 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index 40ca1c4f9fa..b6345b495f1 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -138,7 +138,7 @@ void EnvironmentLoader::Update(const UpdateInfo &,
   if (!this->dataPtr->pub.has_value())
   {
     auto world = worldEntity(_ecm);
-    auto topic = common::joinPaths(scopedName(world, _ecm),"environment");
+    auto topic = common::joinPaths(scopedName(world, _ecm), "environment");
     this->dataPtr->pub =
       {this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
   }
diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index a8cec773d2f..c407da9e00f 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -59,7 +59,7 @@ class EnvironmentVisualizationTool
     auto world = worldEntity(_ecm);
     auto topic =
       common::joinPaths(
-        scopedName(world, _ecm),"environment/visualize/res");
+        scopedName(world, _ecm), "environment/visualize/res");
     std::lock_guard<std::mutex> lock(this->mutex);
     this->pcPub = node.Advertise<gz::msgs::Vector3d>(topic);
   }
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index a746b76b0ed..ef3e0f32ea5 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -18,6 +18,7 @@
 #include "VisualizationTool.hh"
 
 #include <array>
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
@@ -83,7 +84,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   //////////////////////////////////////////////////
   public: EnvironmentPreloadPrivate() :
-    visualizationPtr(new EnvironmentVisualizationTool) {};
+    visualizationPtr(new EnvironmentVisualizationTool) {}
 
   //////////////////////////////////////////////////
   public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg)
@@ -320,10 +321,10 @@ void EnvironmentPreload::PreUpdate(
 
     // See https://github.com/gazebosim/gz-sim/issues/1786
     this->dataPtr->node.Subscribe(
-      common::joinPaths(scopedName(world, _ecm),"environment"),
+      common::joinPaths(scopedName(world, _ecm), "environment"),
       &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get());
     this->dataPtr->node.Subscribe(
-      common::joinPaths(scopedName(world, _ecm),"environment/visualize/res"),
+      common::joinPaths(scopedName(world, _ecm), "environment/visualize/res"),
       &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get());
 
     this->dataPtr->visualizationPtr->resample = true;
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 5bd6a4eef66..816f5bfe0b5 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -18,6 +18,7 @@
 #define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_
 
 #include <atomic>
+#include <memory>
 #include <mutex>
 #include <string>
 #include <unordered_map>

From 7af4af939ebe49bcf1f1ae72e97c4173a51aa115 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 14 Apr 2023 01:55:08 +0000
Subject: [PATCH 15/28] Fix Typo with unit map

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 src/gui/plugins/environment_loader/EnvironmentLoader.cc         | 2 +-
 .../environment_visualization/EnvironmentVisualization.cc       | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index b6345b495f1..0bde8b5dd54 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -87,7 +87,7 @@ class EnvironmentLoaderPrivate
       {QString("degree"),
         Units::DataLoadPathOptions_DataAngularUnits_DEGREES},
       {QString("radians"),
-        Units::DataLoadPathOptions_DataAngularUnits_DEGREES}
+        Units::DataLoadPathOptions_DataAngularUnits_RADIANS}
     };
 
   /// \brief Spatial reference.
diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index c407da9e00f..87ce97a97e0 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -59,7 +59,7 @@ class EnvironmentVisualizationTool
     auto world = worldEntity(_ecm);
     auto topic =
       common::joinPaths(
-        scopedName(world, _ecm), "environment/visualize/res");
+        scopedName(world, _ecm), "environment", "visualize", "res");
     std::lock_guard<std::mutex> lock(this->mutex);
     this->pcPub = node.Advertise<gz::msgs::Vector3d>(topic);
   }

From 57b949ba0b7ddf9efae241cec6ff4c8b6f174aaf Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 14 Apr 2023 03:36:35 +0000
Subject: [PATCH 16/28] Address PR feedback

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../EnvironmentVisualization.cc               |  2 +-
 .../environment_preload/VisualizationTool.cc  | 29 +++++++++----------
 .../environment_preload/VisualizationTool.hh  | 14 ++++-----
 3 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 87ce97a97e0..96e054c3fac 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -86,7 +86,7 @@ class EnvironmentVisualizationTool
   public: std::mutex mutex;
 
   /// \brief first load we need to scan for existing data sensor
-  public: bool first {true};
+  public: bool first{true};
 };
 }
 }
diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index a444ed59920..04e5b924ec5 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -20,13 +20,14 @@
 /////////////////////////////////////////////////
 EnvironmentVisualizationTool::EnvironmentVisualizationTool()
 {
-    this->pcPub =
-      this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
+  this->pcPub =
+    this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
 }
 
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::CreatePointCloudTopics(
-    const std::shared_ptr<components::EnvironmentalData> _data) {
+    const std::shared_ptr<components::EnvironmentalData> &_data)
+{
   this->pubs.clear();
   this->sessions.clear();
 
@@ -49,10 +50,9 @@ void EnvironmentVisualizationTool::FileReloaded()
 void EnvironmentVisualizationTool::Step(
     const UpdateInfo &_info,
     const EntityComponentManager& _ecm,
-    const std::shared_ptr<components::EnvironmentalData> _data,
+    const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples)
 {
-
   auto now = std::chrono::steady_clock::now();
   std::chrono::duration<double> dt(now - this->lastTick);
 
@@ -64,6 +64,7 @@ void EnvironmentVisualizationTool::Step(
     this->lastTick = now;
   }
 
+  // Progress session pointers to next time stamp
   for (auto &it : this->sessions)
   {
     auto res =
@@ -91,14 +92,13 @@ void EnvironmentVisualizationTool::Step(
 
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::Visualize(
-    const std::shared_ptr<components::EnvironmentalData> data,
+    const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples)
 {
-
-  for (auto key : data->frame.Keys())
+  for (auto key : _data->frame.Keys())
   {
     const auto session = this->sessions[key];
-    auto frame = data->frame[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;
@@ -136,7 +136,7 @@ void EnvironmentVisualizationTool::Visualize(
 void EnvironmentVisualizationTool::Publish()
 {
   pcPub.Publish(this->pcMsg);
-  for(auto &[key, pub] : this->pubs)
+  for (auto &[key, pub] : this->pubs)
   {
     pub.Publish(this->floatFields[key]);
   }
@@ -144,11 +144,11 @@ void EnvironmentVisualizationTool::Publish()
 
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::ResizeCloud(
-  const std::shared_ptr<components::EnvironmentalData> _data,
-  const EntityComponentManager& _ecm,
+  const std::shared_ptr<components::EnvironmentalData> &_data,
+  const EntityComponentManager &_ecm,
   unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples)
 {
-  assert (pubs.size() > 0);
+  assert(pubs.size() > 0);
 
   // Assume all data have same point cloud.
   gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
@@ -162,8 +162,7 @@ void EnvironmentVisualizationTool::ResizeCloud(
 
   auto session = this->sessions[this->pubs.begin()->first];
   auto frame = _data->frame[this->pubs.begin()->first];
-  auto [lower_bound, upper_bound] =
-    frame.Bounds(session);
+  auto [lower_bound, upper_bound] = frame.Bounds(session);
 
   auto step = upper_bound - lower_bound;
   auto dx = step.X() / _xSamples;
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index 816f5bfe0b5..de715682e66 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -59,7 +59,7 @@ class EnvironmentVisualizationTool
   private: std::mutex mutex;
 
   /// \brief First load we need to scan for existing data sensor
-  private: bool first {true};
+  private: bool first{true};
 
   /// \brief Enable resampling
   public: std::atomic<bool> resample{true};
@@ -71,7 +71,7 @@ class EnvironmentVisualizationTool
   /// available.
   /// \param[in] _data Data to be visuallized
   private: void CreatePointCloudTopics(
-    const std::shared_ptr<components::EnvironmentalData> _data);
+    const std::shared_ptr<components::EnvironmentalData> &_data);
 
   /// \brief Invoke when new file is made available.
   public: void FileReloaded();
@@ -85,8 +85,8 @@ class EnvironmentVisualizationTool
   /// \param[in] _zSample Samples along z
   public: void Step(
     const UpdateInfo &_info,
-    const EntityComponentManager& _ecm,
-    const std::shared_ptr<components::EnvironmentalData> _data,
+    const EntityComponentManager &_ecm,
+    const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples);
 
   /// \brief Publishes a sample of the data
@@ -95,7 +95,7 @@ class EnvironmentVisualizationTool
   /// \param[in] _ySample Samples along y
   /// \param[in] _zSample Samples along z
   private: void Visualize(
-    const std::shared_ptr<components::EnvironmentalData> data,
+    const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples);
 
   /// \brief Get the point cloud data.
@@ -109,8 +109,8 @@ class EnvironmentVisualizationTool
   /// \param[in] _ySample Samples along y
   /// \param[in] _zSample Samples along z
   private: void ResizeCloud(
-    const std::shared_ptr<components::EnvironmentalData> _data,
-    const EntityComponentManager& _ecm,
+    const std::shared_ptr<components::EnvironmentalData> &_data,
+    const EntityComponentManager &_ecm,
     unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples);
 
   /// \brief Publisher for point clouds

From afcf5c6aee0cf1f16d710aecf1179ae2e7f4e07d Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 17 Apr 2023 02:20:48 +0000
Subject: [PATCH 17/28] Style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/EnvironmentPreload.cc | 23 ++++++++++---------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index ef3e0f32ea5..d4963164829 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -107,7 +107,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
       return;
     }
     auto converted = msgs::Convert(_resChanged);
-    if (this->samples  == converted)
+    if (this->samples == converted)
     {
       // If the sample has not changed return.
       // This is because resampling is expensive.
@@ -123,7 +123,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
   {
     if (!this->sdf->HasElement("data"))
     {
-      gzwarn << "No environmental data file was specified";
+      gzerr << "No environmental data file was specified" << std::endl;
       return;
     }
 
@@ -131,9 +131,9 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::string dataPath =
         this->sdf->Get<std::string>("data");
     this->dataDescription.set_path(dataPath);
-    if (common::isRelativePath(dataDescription.path()))
+    if (common::isRelativePath(this->dataDescription.path()))
     {
-      auto * component =
+      auto *component =
           _ecm.Component<components::WorldSdf>(worldEntity(_ecm));
       const std::string rootPath =
           common::parentPath(component->Data().Element()->FilePath());
@@ -216,20 +216,21 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     this->dataDescription.set_y(spatialColumnNames[1]);
     this->dataDescription.set_z(spatialColumnNames[2]);
 
-    needsReload = true;
+    this->needsReload = true;
   }
 
   //////////////////////////////////////////////////
   public: components::EnvironmentalData::ReferenceUnits ConvertUnits(
     const Units &_unit)
   {
-    switch (_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" << std::endl;
+        gzerr << "Invalid unit conversion. Defaulting to radians." << std::endl;
         return components::EnvironmentalData::ReferenceUnits::RADIANS;
     }
   }
@@ -252,7 +253,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
       std::ifstream dataFile(this->dataDescription.path());
       if (!dataFile.is_open())
       {
-        if(logFileLoadError)
+        if (this->logFileLoadError)
         {
           gzerr << "No environmental data file was found at " <<
             this->dataDescription.path() << std::endl;
@@ -279,15 +280,15 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     }
     catch (const std::invalid_argument &exc)
     {
-      if(logFileLoadError)
+      if (this->logFileLoadError)
       {
         gzerr << "Failed to load environment data" << std::endl
               << exc.what() << std::endl;
-        logFileLoadError = false;
+        this->logFileLoadError = false;
       }
     }
 
-    needsReload = false;
+    this->needsReload = false;
   }
 };
 

From 6b56a43e20666bccadbb6f6db817b2ca2d7dd611 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 17 Apr 2023 02:32:51 +0000
Subject: [PATCH 18/28] Fix incorrect use of path.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 src/systems/environment_preload/EnvironmentPreload.cc | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index d4963164829..5e8f5266306 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -322,10 +322,12 @@ void EnvironmentPreload::PreUpdate(
 
     // See https://github.com/gazebosim/gz-sim/issues/1786
     this->dataPtr->node.Subscribe(
-      common::joinPaths(scopedName(world, _ecm), "environment"),
+      transport::TopicUtils::AsValidTopic(
+        scopedName(world, _ecm) + "/environment"),
       &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get());
     this->dataPtr->node.Subscribe(
-      common::joinPaths(scopedName(world, _ecm), "environment/visualize/res"),
+      transport::TopicUtils::AsValidTopic(
+        scopedName(world, _ecm) + "/environment/visualize/res"),
       &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get());
 
     this->dataPtr->visualizationPtr->resample = true;

From c2708db313f9808a359a7755b2977e93c082f191 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Tue, 18 Jul 2023 07:04:43 +0000
Subject: [PATCH 19/28] Fix example loading issues.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 examples/worlds/CMakeLists.txt                        |  4 ++++
 examples/worlds/environmental_sensor.sdf              | 11 ++++++++++-
 src/systems/environment_preload/EnvironmentPreload.cc | 10 ++++++----
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/examples/worlds/CMakeLists.txt b/examples/worlds/CMakeLists.txt
index fee3a9e41c0..258ee1529eb 100644
--- a/examples/worlds/CMakeLists.txt
+++ b/examples/worlds/CMakeLists.txt
@@ -2,4 +2,8 @@ file(GLOB files "*.sdf")
 install(FILES ${files}
   DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds)
 
+file(GLOB csv_files "*.csv")
+install(FILES ${csv_files}
+  DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds)
+
 add_subdirectory(thumbnails)
diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf
index e1947c85911..945e89bd4d7 100644
--- a/examples/worlds/environmental_sensor.sdf
+++ b/examples/worlds/environmental_sensor.sdf
@@ -1,5 +1,12 @@
 <?xml version="1.0" ?>
-<!-- This example show cases how to load and unload environmental data. -->
+<!-- 
+  This example show cases how to load and unload environmental data.
+  Before opening this file in a seperate terminal run:
+    gz topic -e -t /sensors/humidity
+  Then open this file by running:
+    gz sim environmental_sensor.sdf
+  You should see a data stream of increasing numbers eventually stop at 90. 
+-->
 <sdf version="1.6">
   <world name="environmental_sensor_example">
     <plugin
@@ -20,6 +27,7 @@
       name="gz::sim::systems::EnvironmentalSystem">
     </plugin>
 
+    <!-- The system specifies where to preload -->
     <plugin
       filename="gz-sim-environment-preload-system"
       name="gz::sim::systems::EnvironmentPreload">
@@ -104,6 +112,7 @@
         <sensor name="custom_sensor" type="custom" gz:type="environmental_sensor/humidity">
           <always_on>1</always_on>
           <update_rate>30</update_rate>
+          <topic>sensors/humidity</topic>
         </sensor>
       </link>
     </model>
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 5e8f5266306..1dcb678c5a9 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -130,15 +130,16 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::lock_guard<std::mutex> lock(mtx);
     std::string dataPath =
         this->sdf->Get<std::string>("data");
-    this->dataDescription.set_path(dataPath);
+    
     if (common::isRelativePath(this->dataDescription.path()))
     {
       auto *component =
           _ecm.Component<components::WorldSdf>(worldEntity(_ecm));
       const std::string rootPath =
           common::parentPath(component->Data().Element()->FilePath());
-      dataPath = common::joinPaths(rootPath, this->dataDescription.path());
+      dataPath = common::joinPaths(rootPath, dataPath);
     }
+    this->dataDescription.set_path(dataPath);
 
     this->dataDescription.set_units(
       Units::DataLoadPathOptions_DataAngularUnits_RADIANS);
@@ -262,7 +263,9 @@ class gz::sim::systems::EnvironmentPreloadPrivate
         return;
       }
 
-      gzmsg << "Loading Environment Data\n";
+      gzmsg << "Loading Environment Data " << this->dataDescription.path() <<
+        std::endl;
+
       using ComponentDataT = components::EnvironmentalData;
       auto data = ComponentDataT::MakeShared(
           common::IO<ComponentDataT::FrameT>::ReadFrom(
@@ -271,7 +274,6 @@ class gz::sim::systems::EnvironmentPreloadPrivate
               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));

From 54c42b25e7e3bd9cdd2a680f7ffda24a533aaebc Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Tue, 18 Jul 2023 08:50:17 +0000
Subject: [PATCH 20/28] style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 examples/worlds/environmental_sensor.sdf              | 4 ++--
 src/systems/environment_preload/EnvironmentPreload.cc | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf
index 945e89bd4d7..2d25fb9dbe1 100644
--- a/examples/worlds/environmental_sensor.sdf
+++ b/examples/worlds/environmental_sensor.sdf
@@ -1,11 +1,11 @@
 <?xml version="1.0" ?>
-<!-- 
+<!--
   This example show cases how to load and unload environmental data.
   Before opening this file in a seperate terminal run:
     gz topic -e -t /sensors/humidity
   Then open this file by running:
     gz sim environmental_sensor.sdf
-  You should see a data stream of increasing numbers eventually stop at 90. 
+  You should see a data stream of increasing numbers eventually stop at 90.
 -->
 <sdf version="1.6">
   <world name="environmental_sensor_example">
diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 1dcb678c5a9..ae395241897 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -130,7 +130,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::lock_guard<std::mutex> lock(mtx);
     std::string dataPath =
         this->sdf->Get<std::string>("data");
-    
+
     if (common::isRelativePath(this->dataDescription.path()))
     {
       auto *component =

From fe1bc7c98cda957e219d6ff6acc9a0a36d99aa56 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Tue, 22 Aug 2023 20:59:52 +0800
Subject: [PATCH 21/28] Update
 src/systems/environment_preload/VisualizationTool.cc

Co-authored-by: Mabel Zhang <mabel@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
---
 src/systems/environment_preload/VisualizationTool.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index 04e5b924ec5..e2df135f100 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -49,7 +49,7 @@ void EnvironmentVisualizationTool::FileReloaded()
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::Step(
     const UpdateInfo &_info,
-    const EntityComponentManager& _ecm,
+    const EntityComponentManager &_ecm,
     const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples)
 {

From 3cf7896364ab02b2ad7de9b8b08e19d6aca9fa25 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Thu, 24 Aug 2023 08:01:44 +0000
Subject: [PATCH 22/28] Adds a warning regarding loading plugins.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../plugins/environment_loader/EnvironmentLoader.cc   | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index 0bde8b5dd54..d6c14317956 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -138,10 +138,19 @@ void EnvironmentLoader::Update(const UpdateInfo &,
   if (!this->dataPtr->pub.has_value())
   {
     auto world = worldEntity(_ecm);
-    auto topic = common::joinPaths(scopedName(world, _ecm), "environment");
+    auto topic = scopedName(world, _ecm) + "/" + "environment";
     this->dataPtr->pub =
       {this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
   }
+
+  static bool warned = false;
+  if (!this->dataPtr->pub->HasConnections() && !warned)
+  {
+    warned = true;
+    gzwarn << "Could not find a subscriber for the environment "
+      << "make sure to load the Environment Preload plugin"
+      << std::endl;
+  }
 }
 
 /////////////////////////////////////////////////

From 6b2c398c70401bec8d4171203ccb401b9d1985d8 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 25 Aug 2023 05:52:26 +0000
Subject: [PATCH 23/28] Automatically loads plugin if missing

This commit automatically loads the environment preload plugin if it is
missing.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_loader/EnvironmentLoader.cc   | 49 +++++++++++++++++--
 1 file changed, 44 insertions(+), 5 deletions(-)

diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
index d6c14317956..cb688b3a8ae 100644
--- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc
+++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc
@@ -20,8 +20,10 @@
 #include <gz/gui/Application.hh>
 #include <gz/gui/MainWindow.hh>
 #include <gz/sim/Util.hh>
+#include <gz/sim/components/Name.hh>
 
 #include <gz/plugin/Register.hh>
+#include <gz/msgs/entity_plugin_v.pb.h>
 
 #include <atomic>
 #include <mutex>
@@ -46,7 +48,10 @@ namespace sim
 {
 inline namespace GZ_SIM_VERSION_NAMESPACE
 {
-
+const char* preload_plugin_name{
+  "gz::sim::systems::EnvironmentPreload"};
+const char* preload_plugin_filename{
+  "gz-sim-environment-preload-system"};
 using Units = msgs::DataLoadPathOptions_DataAngularUnits;
 /// \brief Private data class for EnvironmentLoader
 class EnvironmentLoaderPrivate
@@ -135,10 +140,12 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *)
 void EnvironmentLoader::Update(const UpdateInfo &,
                                EntityComponentManager &_ecm)
 {
+  auto world = worldEntity(_ecm);
+
   if (!this->dataPtr->pub.has_value())
   {
-    auto world = worldEntity(_ecm);
-    auto topic = scopedName(world, _ecm) + "/" + "environment";
+    auto topic = transport::TopicUtils::AsValidTopic(
+      scopedName(world, _ecm) + "/" + "environment");
     this->dataPtr->pub =
       {this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
   }
@@ -147,9 +154,41 @@ void EnvironmentLoader::Update(const UpdateInfo &,
   if (!this->dataPtr->pub->HasConnections() && !warned)
   {
     warned = true;
-    gzwarn << "Could not find a subscriber for the environment "
-      << "make sure to load the Environment Preload plugin"
+    gzwarn << "Could not find a subscriber for the environment. "
+      << "Attempting to load environmental preload plugin."
       << std::endl;
+
+    auto nameComp = _ecm.Component<components::Name>(world);
+    if (nullptr == nameComp) {
+      gzerr << "Failed to get world name" << std::endl;
+      return;
+    }
+    auto worldName = nameComp->Data();
+    msgs::EntityPlugin_V req;
+    req.mutable_entity()->set_id(world);
+    auto plugin = req.add_plugins();
+    plugin->set_name(preload_plugin_name);
+    plugin->set_filename(preload_plugin_filename);
+    plugin->set_innerxml("");
+    msgs::Boolean res;
+    bool result;
+    const unsigned int timeout = 5000;
+    const auto service = transport::TopicUtils::AsValidTopic(
+      "/world/" + worldName + "/entity/system/add");
+    if (service.empty())
+    {
+      gzerr << "Unable to request " << service << std::endl;
+      return;
+    }
+
+    if (this->dataPtr->node.Request(service, req, timeout, res, result))
+    {
+      gzdbg << "Added plugin successfully" << std::endl;
+    }
+    else
+    {
+      gzerr << "Failed to load plugin" << std::endl;
+    }
   }
 }
 

From 85b6eac9acccd3b107b4507955332361399836f9 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 25 Aug 2023 06:44:29 +0000
Subject: [PATCH 24/28] Address some feedback I missed

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/VisualizationTool.cc  | 21 ++++++++++---------
 1 file changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index e2df135f100..fb24349fa3f 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -146,14 +146,16 @@ void EnvironmentVisualizationTool::Publish()
 void EnvironmentVisualizationTool::ResizeCloud(
   const std::shared_ptr<components::EnvironmentalData> &_data,
   const EntityComponentManager &_ecm,
-  unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples)
+  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}});
-  auto numberOfPoints = _xSamples * _ySamples * _zSamples;
+  auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples;
   std::size_t dataSize{
     static_cast<std::size_t>(numberOfPoints * pcMsg.point_step())};
   pcMsg.mutable_data()->resize(dataSize);
@@ -165,26 +167,25 @@ void EnvironmentVisualizationTool::ResizeCloud(
   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;
+  auto dx = step.X() / _numXSamples;
+  auto dy = step.Y() / _numYSamples;
+  auto dz = step.Z() / _numZSamples;
 
   // Populate point cloud
   gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
   gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
   gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");
 
-  for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++)
+  for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++)
   {
     auto x = lower_bound.X() + x_steps * dx;
-    for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++)
+    for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++)
     {
       auto y = lower_bound.Y() + y_steps * dy;
-      for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++)
+      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},
+        auto coords = getGridFieldCoordinates(_ecm, math::Vector3d{x, y, z},
           _data);
 
         if (!coords.has_value())

From 04a3858f7c7c5200e76326cde0acfa1f464e8079 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Fri, 25 Aug 2023 23:32:19 +0000
Subject: [PATCH 25/28] Address some feedback

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 examples/worlds/environmental_sensor.sdf                        | 2 +-
 .../environment_visualization/EnvironmentVisualization.cc       | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf
index 2d25fb9dbe1..8ef8ed35f1b 100644
--- a/examples/worlds/environmental_sensor.sdf
+++ b/examples/worlds/environmental_sensor.sdf
@@ -5,7 +5,7 @@
     gz topic -e -t /sensors/humidity
   Then open this file by running:
     gz sim environmental_sensor.sdf
-  You should see a data stream of increasing numbers eventually stop at 90.
+  Play the simulation and you should see a data stream of increasing numbers eventually stop at 90.
 -->
 <sdf version="1.6">
   <world name="environmental_sensor_example">
diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
index 96e054c3fac..c9c040f4541 100644
--- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
+++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc
@@ -119,7 +119,7 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *)
 }
 
 /////////////////////////////////////////////////
-void EnvironmentVisualization::Update(const UpdateInfo &_info,
+void EnvironmentVisualization::Update(const UpdateInfo &,
                                EntityComponentManager &_ecm)
 {
   if (this->dataPtr->first)

From e3ed11a871fbee77252d7200ae42f8ad3e2dad91 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 28 Aug 2023 10:56:47 +0000
Subject: [PATCH 26/28] Fixes issue  described by @iche033.

However fix depends on https://github.com/gazebosim/gz-math/pull/551

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 .../environment_preload/VisualizationTool.cc  | 27 ++++++++++++++++---
 .../environment_preload/VisualizationTool.hh  |  4 ++-
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index fb24349fa3f..42ca2ad12ee 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -26,7 +26,8 @@ EnvironmentVisualizationTool::EnvironmentVisualizationTool()
 
 /////////////////////////////////////////////////
 void EnvironmentVisualizationTool::CreatePointCloudTopics(
-    const std::shared_ptr<components::EnvironmentalData> &_data)
+    const std::shared_ptr<components::EnvironmentalData> &_data,
+    const UpdateInfo &_info)
 {
   this->pubs.clear();
   this->sessions.clear();
@@ -36,7 +37,16 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics(
     this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
     gz::msgs::Float_V msg;
     this->floatFields.emplace(key, msg);
-    this->sessions.emplace(key, _data->frame[key].CreateSession());
+    const double time = std::chrono::duration<double>(_info.simTime).count();
+    auto sess = _data->frame[key].CreateSession(time);
+    if (!_data->frame[key].IsValid(sess))
+    {
+      gzerr << key << "data is out of time bounds. Nothing will be published"
+        <<std::endl;
+      this->finishedTime = true;
+      continue;
+    }
+    this->sessions.emplace(key, sess);
   }
 }
 
@@ -53,12 +63,20 @@ void EnvironmentVisualizationTool::Step(
     const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples)
 {
+  if (this->finishedTime) 
+  {
+    return;
+  }
   auto now = std::chrono::steady_clock::now();
   std::chrono::duration<double> dt(now - this->lastTick);
 
   if (this->resample)
   {
-    this->CreatePointCloudTopics(_data);
+    this->CreatePointCloudTopics(_data, _info);
+    if (this->finishedTime) {
+      this->resample = false;
+      return;
+    }
     this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples);
     this->resample = false;
     this->lastTick = now;
@@ -76,6 +94,9 @@ void EnvironmentVisualizationTool::Step(
     }
     else
     {
+      gzerr << "Data does not exist beyond this time."
+        << " Not publishing new environment visuallization data."
+        << std::endl;
       this->finishedTime = true;
       return;
     }
diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh
index de715682e66..bb2e2fce1b4 100644
--- a/src/systems/environment_preload/VisualizationTool.hh
+++ b/src/systems/environment_preload/VisualizationTool.hh
@@ -70,8 +70,10 @@ class EnvironmentVisualizationTool
   /// \brief Create publisher structures whenever a new environment is made
   /// available.
   /// \param[in] _data Data to be visuallized
+  /// \param[in] _info simulation info for current time step
   private: void CreatePointCloudTopics(
-    const std::shared_ptr<components::EnvironmentalData> &_data);
+    const std::shared_ptr<components::EnvironmentalData> &_data,
+    const UpdateInfo &_info);
 
   /// \brief Invoke when new file is made available.
   public: void FileReloaded();

From 4d0a03468e0662b300e5c2a45e9609cf8bc01f84 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 28 Aug 2023 11:16:18 +0000
Subject: [PATCH 27/28] style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 src/systems/environment_preload/VisualizationTool.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc
index 42ca2ad12ee..75b37b65571 100644
--- a/src/systems/environment_preload/VisualizationTool.cc
+++ b/src/systems/environment_preload/VisualizationTool.cc
@@ -63,7 +63,7 @@ void EnvironmentVisualizationTool::Step(
     const std::shared_ptr<components::EnvironmentalData> &_data,
     double _xSamples, double _ySamples, double _zSamples)
 {
-  if (this->finishedTime) 
+  if (this->finishedTime)
   {
     return;
   }

From 0672777faf8f2c068fbcf2e55a38d4d12511b2b5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Mon, 4 Sep 2023 02:04:20 +0000
Subject: [PATCH 28/28] Fixed failing tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 src/systems/environment_preload/EnvironmentPreload.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc
index 16384545457..5192fa46dfb 100644
--- a/src/systems/environment_preload/EnvironmentPreload.cc
+++ b/src/systems/environment_preload/EnvironmentPreload.cc
@@ -131,7 +131,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
     std::string dataPath =
         this->sdf->Get<std::string>("data");
 
-    if (common::isRelativePath(this->dataDescription.path()))
+    if (common::isRelativePath(dataPath))
     {
       auto *component =
           _ecm.Component<components::WorldSdf>(worldEntity(_ecm));