Sprinkled with healthy dose of Doxygen

Also refactored the visualization tool out. Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
gazebosim · arjo129 · Oct 3, 2023 · Dec 20, 2022 · Dec 21, 2022 · Jan 12, 2023
commit ea93a1bb7d435a77109469e54d058dfae80e629d
@@ -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.

@@ -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

@@ -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;
       }
     }
 

@@ -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(""));
+  }
+}