Fix enviroment system loading mechanism (#1842)

* 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.

Signed-off-by: Arjo Chakravarty <[email protected]>

* small changes

Signed-off-by: Arjo Chakravarty <[email protected]>

* Working on porting the visuals

Signed-off-by: Arjo Chakravarty <[email protected]>

* Actually send message for loading from ui to environment preload plugin.

Visuallization still goes 💥

Signed-off-by: Arjo Chakravarty <[email protected]>

* Rewrite EnvironmentVisualization Widget to be simpler.

Signed-off-by: Arjo Chakravarty <[email protected]>

* fix crashes.

Vis still not working

Signed-off-by: Arjo Chakravarty <[email protected]>

* Get a different 💥

Signed-off-by: Arjo Chakravarty <[email protected]>

* Works some times.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fixed synchronization issues.

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

Signed-off-by: Arjo Chakravarty <[email protected]>

* No more 💥s 🎉

Signed-off-by: Arjo Chakravarty <[email protected]>

* style

Signed-off-by: Arjo Chakravarty <[email protected]>

* Sprinkled with healthy dose of Doxygen

Also refactored the visualization tool out.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Style

Signed-off-by: Arjo Chakravarty <[email protected]>

* More style fixes

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fix Typo with unit map

Signed-off-by: Arjo Chakravarty <[email protected]>

* Address PR feedback

Signed-off-by: Arjo Chakravarty <[email protected]>

* Style fixes

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fix incorrect use of path.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fix example loading issues.

Signed-off-by: Arjo Chakravarty <[email protected]>

* style

Signed-off-by: Arjo Chakravarty <[email protected]>

* Update src/systems/environment_preload/VisualizationTool.cc

Co-authored-by: Mabel Zhang <[email protected]>
Signed-off-by: Arjo Chakravarty <[email protected]>

* Adds a warning regarding loading plugins.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Automatically loads plugin if missing

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

Signed-off-by: Arjo Chakravarty <[email protected]>

* Address some feedback I missed

Signed-off-by: Arjo Chakravarty <[email protected]>

* Address some feedback

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fixes issue  described by @iche033.

However fix depends on gazebosim/gz-math#551

Signed-off-by: Arjo Chakravarty <[email protected]>

* style

Signed-off-by: Arjo Chakravarty <[email protected]>

* Fixed failing tests

Signed-off-by: Arjo Chakravarty <[email protected]>

---------

Signed-off-by: Arjo Chakravarty <[email protected]>
Signed-off-by: Arjo Chakravarty <[email protected]>
Signed-off-by: Arjo Chakravarty <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>
Co-authored-by: Mabel Zhang <[email protected]>
Co-authored-by: Ian Chen <[email protected]>

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)

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
+ 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">
  <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>

src/gui/plugins/environment_loader/EnvironmentLoader.cc

@@ -19,10 +19,11 @@
 
 #include <gz/gui/Application.hh>
 #include <gz/gui/MainWindow.hh>
-#include <gz/sim/components/Environment.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>
@@ -33,6 +34,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 +48,11 @@ 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
 {
@@ -65,7 +76,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 +87,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_RADIANS}
  };
 
  /// \brief Spatial reference.
@@ -92,6 +103,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 +140,80 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *)
 void EnvironmentLoader::Update(const UpdateInfo &,
  EntityComponentManager &_ecm)
 {
- if (this->dataPtr->needsLoad)
+ auto world = worldEntity(_ecm);
+
+ 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
+ auto topic = transport::TopicUtils::AsValidTopic(
+ 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. "
+ << "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))
  {
- 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)});
+ gzdbg << "Added plugin successfully" << std::endl;
  }
- catch (const std::invalid_argument &exc)
+ else
  {
- gzerr << "Failed to load environmental data" << std::endl
- << exc.what() << std::endl;
+ gzerr << "Failed to load plugin" << std::endl;
  }
  }
 }
 
 /////////////////////////////////////////////////
 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);
+ }
 }
 
 /////////////////////////////////////////////////