Backport #1748: Adds a tool for environment data visualization and cu…

…stom environmental sensors (#1798)

This PR adds a tool to visualize Scalar Environmental Data. It also adds custom sensors.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

examples/worlds/environmental_data.csv

@@ -0,0 +1,17 @@
+timestamp,humidity,x,y,z
+0,80,-1,-1,-1
+0,80,-1,-1, 1
+0,80,-1, 1,-1
+0,80,-1, 1, 1
+0,90, 1,-1,-1
+0,90, 1,-1, 1
+0,90, 1, 1,-1
+0,90, 1, 1, 1
+1,90,-1,-1,-1
+1,90,-1,-1, 1
+1,90,-1, 1,-1
+1,90,-1, 1, 1
+1,90, 1,-1,-1
+1,90, 1,-1, 1
+1,90, 1, 1,-1
+1,90, 1, 1, 1

examples/worlds/environmental_sensor.sdf

@@ -0,0 +1,112 @@
+<?xml version="1.0" ?>
+<!-- This example show cases how to load and unload environmental data. -->
+<sdf version="1.6">
+  <world name="environmental_sensor_example">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <!-- The system is added to the world, so it handles all environmental data in the world-->
+    <plugin
+      filename="gz-sim-environmental-sensor-system"
+      name="gz::sim::systems::EnvironmentalSystem">
+    </plugin>
+
+    <plugin
+      filename="gz-sim-environment-preload-system"
+      name="gz::sim::systems::EnvironmentPreload">
+      <data>environmental_data.csv</data>
+      <dimensions>
+        <time>timestamp</time>
+        <space>
+          <x>x</x>
+          <y>y</y>
+          <z>z</z>
+        </space>
+      </dimensions>
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="model_with_sensor">
+      <pose>0 0 0.05 0 0 0</pose>
+      <link name="link">
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <!-- Here's our custom sensor -->
+        <sensor name="custom_sensor" type="custom" gz:type="environmental_sensor/humidity">
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+        </sensor>
+      </link>
+    </model>
+
+  </world>
+</sdf>

include/gz/sim/Util.hh

@@ -19,17 +19,21 @@
 
 #include <gz/msgs/entity.pb.h>
 
+#include <memory>
 #include <string>
 #include <unordered_set>
 #include <vector>
 
 #include <gz/math/Pose3.hh>
+
+#include "gz/sim/components/Environment.hh"
 #include "gz/sim/config.hh"
 #include "gz/sim/Entity.hh"
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Export.hh"
 #include "gz/sim/Types.hh"
 
+
 namespace gz
 {
   namespace sim
@@ -288,6 +292,16 @@ namespace gz
     std::optional<math::Vector3d> GZ_SIM_VISIBLE sphericalCoordinates(
         Entity _entity, const EntityComponentManager &_ecm);
 
+    /// \brief Get grid field coordinates based on a world position in cartesian
+    /// coordinate frames.
+    /// \param[in] _ecm Entity Component Manager
+    /// \param[in] _worldPosition world position
+    /// \param[in] _gridField Gridfield you are interested in.
+    std::optional<math::Vector3d> GZ_SIM_VISIBLE getGridFieldCoordinates(
+      const EntityComponentManager &_ecm,
+      const math::Vector3d& _worldPosition,
+      const std::shared_ptr<components::EnvironmentalData>& _gridField);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"};
 

include/gz/sim/components/Environment.hh

@@ -46,20 +46,34 @@ namespace components
     using FrameT = common::DataFrame<std::string, T>;
     using ReferenceT = math::SphericalCoordinates::CoordinateType;
 
+    /// \brief Reference units
+    enum class ReferenceUnits {
+      RADIANS = 0,
+      DEGREES
+    };
+
     /// \brief Instantiate environmental data.
     ///
     /// An std::make_shared equivalent that ensures
     /// dynamically loaded call sites use a template
     /// instantiation that is guaranteed to outlive
     /// them.
     static std::shared_ptr<EnvironmentalData>
-    MakeShared(FrameT _frame, ReferenceT _reference);
+    MakeShared(FrameT _frame, ReferenceT _reference,
+      ReferenceUnits _units = ReferenceUnits::RADIANS,
+      bool _ignoreTimeStep = false);
 
     /// \brief Environmental data frame.
     FrameT frame;
 
     /// \brief Spatial reference for data coordinates.
     ReferenceT reference;
+
+    /// \brief The units to be used (only for spherical coordinates)
+    ReferenceUnits units;
+
+    /// \brief Use time axis or not.
+    bool staticTime;
   };
 
   /// \brief A component type that contains a environment data.

src/Util.cc

@@ -30,6 +30,7 @@
 
 #include "gz/sim/components/Actor.hh"
 #include "gz/sim/components/Collision.hh"
+#include "gz/sim/components/Environment.hh"
 #include "gz/sim/components/Joint.hh"
 #include "gz/sim/components/Light.hh"
 #include "gz/sim/components/Link.hh"
@@ -671,6 +672,45 @@ std::optional<math::Vector3d> sphericalCoordinates(Entity _entity,
   return math::Vector3d(GZ_RTOD(rad.X()), GZ_RTOD(rad.Y()), rad.Z());
 }
 
+//////////////////////////////////////////////////
+std::optional<math::Vector3d> getGridFieldCoordinates(
+  const EntityComponentManager &_ecm,
+  const math::Vector3d& _worldPosition,
+  const std::shared_ptr<components::EnvironmentalData>& _gridField
+  )
+{
+
+  auto origin =
+    _ecm.Component<components::SphericalCoordinates>(worldEntity(_ecm));
+  if (!origin)
+  {
+    if (_gridField->reference == math::SphericalCoordinates::SPHERICAL)
+    {
+      // If the reference frame is spherical, we must have some world reference
+      // coordinates.
+      gzerr << "World has no spherical coordinates,"
+          << " but data was loaded with spherical reference plane"
+          << std::endl;
+      return std::nullopt;
+    }
+    else
+    {
+      // No need to transform
+      return _worldPosition;
+    }
+  }
+  auto position = origin->Data().PositionTransform(
+      _worldPosition, math::SphericalCoordinates::LOCAL2,
+      _gridField->reference);
+  if (_gridField->reference == math::SphericalCoordinates::SPHERICAL &&
+    _gridField->units == components::EnvironmentalData::ReferenceUnits::DEGREES)
+  {
+    position.X(GZ_RTOD(position.X()));
+    position.Y(GZ_RTOD(position.Y()));
+  }
+  return position;
+}
+
 //////////////////////////////////////////////////
 // Getting the first .sdf file in the path
 std::string findFuelResourceSdf(const std::string &_path)

src/components/Environment.cc

@@ -23,10 +23,13 @@
 using namespace gz::sim::components;
 
 std::shared_ptr<EnvironmentalData>
-EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference)
+EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference,
+  ReferenceUnits _units, bool _ignoreTimeStep)
 {
   auto data = std::make_shared<EnvironmentalData>();
   data->frame = std::move(_frame);
   data->reference = _reference;
+  data->units = _units;
+  data->staticTime = _ignoreTimeStep;
   return data;
 }

src/gui/plugins/CMakeLists.txt

@@ -136,6 +136,7 @@ add_subdirectory(copy_paste)
 add_subdirectory(entity_context_menu)
 add_subdirectory(entity_tree)
 add_subdirectory(environment_loader)
+add_subdirectory(environment_visualization)
 add_subdirectory(joint_position_controller)
 add_subdirectory(lights)
 add_subdirectory(playback_scrubber)

src/gui/plugins/environment_loader/EnvironmentLoader.cc

@@ -64,6 +64,9 @@ class EnvironmentLoaderPrivate
   /// \brief Index of data dimension to be used as z coordinate.
   public: int zIndex{-1};
 
+  /// \brief Index of data dimension to be used as units.
+  public: QString unit;
+
   public: using ReferenceT = math::SphericalCoordinates::CoordinateType;
 
   /// \brief Map of supported spatial references.
@@ -72,6 +75,15 @@ class EnvironmentLoaderPrivate
     {QString("spherical"), math::SphericalCoordinates::SPHERICAL},
     {QString("ecef"), math::SphericalCoordinates::ECEF}};
 
+  /// \brief Map of supported spatial units.
+  public: const QMap<QString, components::EnvironmentalData::ReferenceUnits>
+    unitMap{
+      {QString("degree"),
+        components::EnvironmentalData::ReferenceUnits::DEGREES},
+      {QString("radians"),
+        components::EnvironmentalData::ReferenceUnits::RADIANS}
+    };
+
   /// \brief Spatial reference.
   public: QString reference;
 
@@ -116,6 +128,8 @@ void EnvironmentLoader::Update(const UpdateInfo &,
     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()
@@ -131,7 +145,8 @@ void EnvironmentLoader::Update(const UpdateInfo &,
                 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->referenceMap[this->dataPtr->reference],
+          this->dataPtr->unitMap[this->dataPtr->unit]);
 
       using ComponentT = components::Environment;
       _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)});
@@ -210,6 +225,29 @@ QStringList EnvironmentLoader::DimensionList() const
   return this->dataPtr->dimensionList;
 }
 
+/////////////////////////////////////////////////
+QStringList EnvironmentLoader::UnitList() const
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  return this->dataPtr->unitMap.keys();
+}
+
+/////////////////////////////////////////////////
+QString EnvironmentLoader::Unit() const
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  return this->dataPtr->unit;
+}
+
+/////////////////////////////////////////////////
+void EnvironmentLoader::SetUnit(QString _unit)
+{
+  {
+    std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+    this->dataPtr->unit = _unit;
+  }
+  this->IsConfiguredChanged();
+}
 /////////////////////////////////////////////////
 int EnvironmentLoader::TimeIndex() const
 {