Fix enviroment system loading mechanism

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,37 @@ 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]);
+
+    this->dataPtr->pub->Publish(data);
+  }
 }
 
 /////////////////////////////////////////////////