Adds a tool for environment visuallization.

Depends on:
* gazebosim/gz-math#508
* gazebosim/gz-common#446
Work in progress.

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

include/gz/sim/Util.hh

@@ -24,12 +24,15 @@
 #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 +291,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 - The Entity Component Manager
+    /// \param[in] _worldPosition - The world position
+    /// \param[in] _gridField - The gridfield you are interested in.
+    std::optional<math::Vector3d> 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"};
 

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,43 @@ 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
+  )
+{
+  if (_gridField->reference == math::SphericalCoordinates::SPHERICAL)
+  {
+      auto origin =
+        _ecm.Component<components::SphericalCoordinates>(worldEntity(_ecm));
+      if (!origin)
+      {
+        gzerr << "World has no spherical coordinates,"
+            <<" but data was loaded with spherical reference plane" 
+            << std::endl;
+        return std::nullopt;
+      }
+      auto position = origin->Data().PositionTransform(
+          _worldPosition, math::SphericalCoordinates::LOCAL2,
+          _gridField->reference);
+      if (_gridField->units ==
+        components::EnvironmentalData::ReferenceUnits::DEGREES)
+      {
+        position = math::Vector3d{
+          GZ_RTOD(position.X()),
+          GZ_RTOD(position.Y()),
+          position.Z()
+        };
+      }
+  }
+  else
+  {
+    return _worldPosition;
+  }
+}
+
 //////////////////////////////////////////////////
 // Getting the first .sdf file in the path
 std::string findFuelResourceSdf(const std::string &_path)

src/gui/plugins/environment_visualization/EnvironmentVisualization.cc

@@ -33,6 +33,13 @@
 #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>
+
 using namespace gz;
 using namespace sim;
 
@@ -45,17 +52,203 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
 /// \brief Private data class for EnvironmentVisualization
 class EnvironmentVisualizationPrivate
 {
+
+  public: EnvironmentVisualizationPrivate()
+  {
+    this->pcPub = node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
+  }
   /// \brief To synchronize member access.
   public: std::mutex mutex;
 
-  /// \brief Whether to attempt an environmental data load.
-  public: std::atomic<bool> needsLoad{false};
+  /// \brief first load we need to scan for existing data sensor
+  public: bool first {true};
 
-  /// \brief Setup publishers
-  public: 
+  public: std::atomic<bool> resample{true};
 
-  /// \brief first load we need to scan for existing data sensor
-  public: bool first;
+  /////////////////////////////////////////////////
+  public: void CreatePointCloudTopics(
+    std::shared_ptr<components::EnvironmentalData> data) {
+    this->pubs.clear();
+    auto keys = data->frame.Keys();
+    for (auto key: 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 xResolution, double yResolution, double zResolution)
+  {
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> dt(now - this->lastTick);
+
+    if (this->resample.load())
+    {
+      this->CreatePointCloudTopics(data);
+      this->ResizeCloud(data, _ecm, xResolution, yResolution, zResolution);
+      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)
+    {
+      gzerr << "Visualize" << "\n";
+      this->Visualize(data, xResolution, yResolution, zResolution);
+      gzerr << "Publish" << "\n";
+      this->Publish();
+      lastTick = now;
+    }
+  }
+
+  /////////////////////////////////////////////////
+  public: void Visualize(
+    std::shared_ptr<components::EnvironmentalData> data,
+    double xResolution, double yResolution, double zResolution) {
+
+    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() / xResolution;
+      auto dy = step.Y() / yResolution;
+      auto dz = step.Z() / zResolution;
+      std::size_t idx = 0;
+      for (std::size_t x_steps = 0; x_steps < ceil(xResolution); x_steps++)
+      {
+        for (std::size_t y_steps = 0; y_steps < ceil(yResolution); y_steps++)
+        {
+          for (std::size_t z_steps = 0; z_steps < ceil(zResolution); z_steps++)
+          {
+            auto x = lower_bound.X() + x_steps * dx;
+            auto y = lower_bound.Y() + y_steps * dy;
+            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()));
+            }
+            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,
+    double xResolution, double yResolution, double zResolution)
+  {
+    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 =
+      ceil(xResolution) * ceil(yResolution) * ceil(zResolution);
+    unsigned int dataSize{numberOfPoints * pcMsg.point_step()};
+    gzerr << "Datasize " << dataSize << "for" << numberOfPoints << "\n";
+
+    pcMsg.mutable_data()->resize(dataSize+1);
+    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() / xResolution;
+    auto dy = step.Y() / yResolution;
+    auto dz = step.Z() / zResolution;
+
+    // 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 < ceil(xResolution); x_steps++)
+    {
+      for (std::size_t y_steps = 0; y_steps < ceil(yResolution); y_steps++)
+      {
+        for (std::size_t z_steps = 0; z_steps < ceil(zResolution); z_steps++)
+        {
+          auto x = lower_bound.X() + x_steps * dx;
+          auto y = lower_bound.Y() + y_steps * dy;
+          auto z = lower_bound.Z() + z_steps * dz;
+
+          auto coords = GetGridFieldCoordinates(
+            _ecm, math::Vector3d{x, y, z},
+            data);
+
+          if (!coords.has_value())
+          {
+            return;
+          }
+
+          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;
 };
 }
 }
@@ -84,22 +277,51 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *)
 }
 
 /////////////////////////////////////////////////
-void EnvironmentVisualization::Update(const UpdateInfo &,
+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)
+  {
+    return;
+  }
+
+  this->dataPtr->Step(
+    _info, environData->Data(), _ecm,
+    this->xResolution, this->yResolution, this->zResolution
+  );
 }
 
+/////////////////////////////////////////////////
+/*void EnvironmentVisualization::xResolutionChanged()
+{
+  this->dataPtr->resample = true;
+}
+
+/////////////////////////////////////////////////
+void EnvironmentVisualization::yResolutionChanged()
+{
+  this->dataPtr->resample = true;
+}
+
+/////////////////////////////////////////////////
+void EnvironmentVisualization::zResolutionChanged()
+{
+  this->dataPtr->resample = true;
+}*/
+
 // Register this plugin
 GZ_ADD_PLUGIN(gz::sim::EnvironmentVisualization, gz::gui::Plugin)

src/gui/plugins/environment_visualization/EnvironmentVisualization.hh

@@ -40,6 +40,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
   {
     Q_OBJECT
 
+    Q_PROPERTY(qreal xResolution MEMBER xResolution NOTIFY xResolutionChanged);
+    Q_PROPERTY(qreal yResolution MEMBER yResolution NOTIFY yResolutionChanged);
+    Q_PROPERTY(qreal zResolution MEMBER zResolution NOTIFY zResolutionChanged);
+
     /// \brief Constructor
     public: EnvironmentVisualization();
 
@@ -53,9 +57,21 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
     public: void Update(const UpdateInfo &,
                         EntityComponentManager &_ecm) override;
 
+    signals: void xResolutionChanged();
+
+    signals: void yResolutionChanged();
+
+    signals: void zResolutionChanged();
+
     /// \internal
     /// \brief Pointer to private data
     private: std::unique_ptr<EnvironmentVisualizationPrivate> dataPtr;
+
+    public: qreal xResolution{10};
+
+    public: qreal yResolution{10};
+
+    public: qreal zResolution{10};
   };
 }
 }