Rewrite EnvironmentVisualization Widget to be simpler.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
gazebosim · arjo129 · Oct 3, 2023 · Dec 20, 2022 · Dec 21, 2022 · Jan 12, 2023
commit 9b684cf5e6a353993e060e20a5fa6b912aab6745
@@ -36,9 +36,7 @@
 
 #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/vector3d.pb.h>
 #include <gz/msgs/Utility.hh>
 
 using namespace gz;
@@ -53,199 +51,38 @@ inline namespace GZ_SIM_VERSION_NAMESPACE
 /// \brief Private data class for EnvironmentVisualization
 class EnvironmentVisualizationTool
 {
-  public: EnvironmentVisualizationTool()
-  {
-    this->pcPub =
-      this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
-  }
-  /// \brief To synchronize member access.
-  public: std::mutex mutex;
-
-  /// \brief first load we need to scan for existing data sensor
-  public: bool first {true};
-
-  public: std::atomic<bool> resample{true};
-
-  /////////////////////////////////////////////////
-  public: void CreatePointCloudTopics(
-    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());
-    }
-  }
 
   /////////////////////////////////////////////////
-  public: void Step(
-    const UpdateInfo &_info,
-    std::shared_ptr<components::EnvironmentalData> &data,
-    const EntityComponentManager& _ecm,
-    double xSamples, double ySamples, double zSamples)
+  public: void Initiallize(
+    const EntityComponentManager &_ecm)
   {
-    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();
-      }
-    }
-
-    // Publish at 2 hz for now. In future make reconfigureable.
-    if (dt.count() > 0.5)
-    {
-      this->Visualize(data, xSamples, ySamples, zSamples);
-      this->Publish();
-      lastTick = now;
-    }
+    auto world = worldEntity(_ecm);
+    auto topic = 
+      common::joinPaths(
+        scopedName(world, _ecm),"environment/visualize/res");
+    std::lock_guard<std::mutex> lock(this->mutex);
+    this->pcPub = node.Advertise<gz::msgs::Vector3d>(topic);
   }
 
   /////////////////////////////////////////////////
   public: void Visualize(
-    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++;
-          }
-        }
-      }
-    }
-  }
-
-  /////////////////////////////////////////////////
-  public: void Publish()
+    double xSamples, double ySamples, double zSamples)
   {
-    pcPub.Publish(this->pcMsg);
-    for(auto &[key, pub] : this->pubs)
-    {
-      pub.Publish(this->floatFields[key]);
-    }
+    std::lock_guard<std::mutex> lock(this->mutex);
+    this->vec = msgs::Convert(math::Vector3d(xSamples, ySamples, zSamples));
+    this->pcPub.Publish(vec);
   }
 
-  /////////////////////////////////////////////////
-  public: void ResizeCloud(
-    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);
+  public: gz::msgs::Vector3d vec;
+  public: transport::Node::Publisher pcPub;
 
-          if (!coords.has_value())
-          {
-            continue;
-          }
+  public: transport::Node node;
 
-          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(""));
-    }
-  }
+  /// \brief To synchronize member access.
+  public: std::mutex mutex;
 
-  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;
+  /// \brief first load we need to scan for existing data sensor
+  public: bool first {true};
 };
 }
 }
@@ -277,35 +114,19 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *)
 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)
+  if (this->dataPtr->first)
   {
-    return;
+    this->dataPtr->Initiallize(_ecm);
+    this->dataPtr->first = false;
   }
-
-  this->dataPtr->Step(
-    _info, environData->Data(), _ecm,
-    this->xSamples, this->ySamples, this->zSamples
-  );
 }
 
 /////////////////////////////////////////////////
 void EnvironmentVisualization::ResamplePointcloud()
 {
-  this->dataPtr->resample = true;
+  this->dataPtr->Visualize(
+    this->xSamples, this->ySamples, this->zSamples
+  );
 }
 
 

@@ -59,9 +59,9 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: std::unique_ptr<EnvironmentVisualizationTool> visualizationPtr;
 
-  public: std::atomic<bool> visualize{false};
+  public: bool visualize{false};
 
-  public: std::atomic<math::Vector3d> samples;
+  public: math::Vector3d samples;
 
   public: bool logError{true};
 
@@ -79,6 +79,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
 
   public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
   {
+    std::lock_guard<std::mutex> lock(this->mtx);
     this->visualize = true;
     this->samples = msgs::Convert(_resChanged);
   }
@@ -298,7 +299,7 @@ void EnvironmentPreload::PreUpdate(
 
   if (this->dataPtr->visualize)
   {
-    auto samples = this->dataPtr->samples.load();
+    auto samples = this->dataPtr->samples;
     this->dataPtr->visualizationPtr->Step(_info, _ecm,
       samples.X(), samples.Y(), samples.Z());
   }

@@ -64,13 +64,21 @@ class EnvironmentVisualizationTool
   /////////////////////////////////////////////////
   public: void CreatePointCloudTopics(
     std::shared_ptr<components::EnvironmentalData> data) {
+
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
     this->pubs.clear();
     this->sessions.clear();
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     for (auto key : data->frame.Keys())
     {
+          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
       gz::msgs::Float_V msg;
       this->floatFields.emplace(key, msg);
+          std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->sessions.emplace(key, data->frame[key].CreateSession());
     }
   }
@@ -81,11 +89,17 @@ class EnvironmentVisualizationTool
     const EntityComponentManager& _ecm,
     double xSamples, double ySamples, double zSamples)
   {
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     auto now = std::chrono::steady_clock::now();
     std::chrono::duration<double> dt(now - this->lastTick);
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     std::shared_ptr<components::EnvironmentalData> data;
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     bool proceed{false};
 
     _ecm.Each<components::Environment>([&](
@@ -94,22 +108,40 @@ class EnvironmentVisualizationTool
     ) -> bool {
       if (_env)
       {
+    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
         data = _env->Data();
         proceed = true;
+    //std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       }
     });
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     if (!proceed)
       return;
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     if (this->resample)
     {
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->CreatePointCloudTopics(data);
+
+
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
       this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples);
       this->resample = false;
       this->lastTick = now;
+        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     }
 
+    std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
     for (auto &it : this->sessions)
     {
       auto res =
@@ -120,6 +152,8 @@ class EnvironmentVisualizationTool
         it.second = res.value();
       }
     }
+        std::cout << __FILE__ << ": " << __LINE__ << std::endl;
+
 
     // Publish at 2 hz for now. In future make reconfigureable.
     if (dt.count() > 0.5)