From 53210d2c1d5a78382936be01075517487fcaad69 Mon Sep 17 00:00:00 2001
From: Rhys Mainwaring <rhys.mainwaring@me.com>
Date: Tue, 21 Feb 2023 10:45:02 +0000
Subject: [PATCH] VisualizeForces: add debug info

- Publish both EntityWrench (single message) and EntityWrenches (map) from Link.
- Process both EntityWrench and EntityWrenches in the VisualizeForces update.
- Toggle #if - #endif to compare update refresh (single message updating, map is not).

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
---
 src/Link.cc                                   | 70 ++++++++++++++++++-
 .../visualize_forces/VisualizeForces.cc       | 47 ++++++++-----
 2 files changed, 98 insertions(+), 19 deletions(-)

diff --git a/src/Link.cc b/src/Link.cc
index d7245e4becf..152bcd70dfa 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -21,7 +21,7 @@
 #include <gz/math/Vector3.hh>
 
 #include <gz/msgs/Utility.hh>
-#include <gz/msgs/wrench.pb.h>
+#include <gz/msgs/entity_wrench.pb.h>
 
 #include "gz/sim/components/AngularAcceleration.hh"
 #include "gz/sim/components/AngularVelocity.hh"
@@ -438,6 +438,8 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
               msgs::Convert(linkWrenchComp->Data().torque()) + _torque);
   }
 
+#if 1
+  // Publish components::EntityWrenches
   if (this->dataPtr->visualizationLabel.has_value())
   {
     auto& label = this->dataPtr->visualizationLabel.value();
@@ -508,6 +510,72 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
     //         << entityWrenchesComp->Data()[label].DebugString() << "\n";
     // }
   }
+#endif
+
+#if 1
+  /// \todo(srmainwaring) - for debugging - publish components::EntityWrenches
+  if (this->dataPtr->visualizationLabel.has_value())
+  {
+    auto& label = this->dataPtr->visualizationLabel.value();
+
+    // Enable required components.
+    enableComponent<components::WorldPose>(_ecm, this->dataPtr->id, true);
+    enableComponent<components::EntityWrench>(_ecm, this->dataPtr->id, true);
+
+    auto entityWrenchComp =
+        _ecm.Component<components::EntityWrench>(this->dataPtr->id);
+    if (!entityWrenchComp)
+    {
+      static bool informed{false};
+      if (!informed)
+      {
+        gzerr << "Failed to retrieve EntityWrench component for link ["
+              << this->dataPtr->id << "] from [" << label << "]\n";
+      }
+      return;
+    }
+
+    // Populate data
+    msgs::EntityWrench msg;
+
+    // Set label
+    {
+      auto data = msg.mutable_header()->add_data();
+      data->set_key("label");
+      data->add_value(label);
+    }
+
+    // Set name
+    {
+      auto data = msg.mutable_header()->add_data();
+      data->set_key("name");
+      if (this->Name(_ecm).has_value())
+      {
+        data->add_value(this->Name(_ecm).value());
+      }
+    }
+
+    // Set entity
+    msg.mutable_entity()->set_id(this->Entity());
+
+    // Set wrench
+    msgs::Set(msg.mutable_wrench()->mutable_force(), _force);
+    msgs::Set(msg.mutable_wrench()->mutable_torque(), _torque);
+
+    // Update map with wrench
+    entityWrenchComp->Data() = msg;
+
+    _ecm.SetChanged(this->dataPtr->id, components::EntityWrench::typeId,
+        ComponentState::PeriodicChange);
+
+    // {
+    //   gzdbg << "Publishing entity wrench for link ["
+    //         << this->dataPtr->id << "]\n"
+    //         << "Label: " << label << "\n"
+    //         << entityWrenchComp->Data().DebugString() << "\n";
+    // }
+  }
+#endif
 }
 
 //////////////////////////////////////////////////
diff --git a/src/gui/plugins/visualize_forces/VisualizeForces.cc b/src/gui/plugins/visualize_forces/VisualizeForces.cc
index 442b99487c7..6ca1366559b 100644
--- a/src/gui/plugins/visualize_forces/VisualizeForces.cc
+++ b/src/gui/plugins/visualize_forces/VisualizeForces.cc
@@ -130,8 +130,10 @@ void VisualizeForcesPrivate::RetrieveWrenchesFromEcm(
 {
   std::lock_guard<std::mutex> lock(mutex);
 
+#if 0
   {
-    _ecm.EachNoCache<components::WorldPose, components::EntityWrenches>(
+    // Read components::EntityWrenches
+    _ecm.Each<components::WorldPose, components::EntityWrenches>(
       [&](const Entity &_entity,
           components::WorldPose *_worldPose,
           components::EntityWrenches *_entityWrenches) -> bool
@@ -160,24 +162,33 @@ void VisualizeForcesPrivate::RetrieveWrenchesFromEcm(
       return true;
     });
   }
+#endif
 
-  // more debugging
-  // {
-  //   msgs::SerializedStateMap state;
-  //   _ecm.ChangedState(state);
-  //   gzdbg << state.DebugString() << "\n";
-  // }
-  // {
-  //   auto comps = _ecm.ComponentTypesWithPeriodicChanges();
-  //   if (comps.find(components::EntityWrenches::typeId) != comps.end())
-  //   {
-  //     gzdbg << "components::EntityWrenches has PeriodicChanges\n";
-  //   }
-  //   else
-  //   {
-  //     gzwarn << "components::EntityWrenches missing PeriodicChanges\n";
-  //   }
-  // }
+#if 1
+  {
+    /// \todo(srmainwaring) - for debugging - read components::EntityWrench
+    _ecm.Each<components::WorldPose, components::EntityWrench>(
+      [&](const Entity &_entity,
+          components::WorldPose *_worldPose,
+          components::EntityWrench *_entityWrench) -> bool
+      {
+      // Push the wrench for this entity to the queue.
+      this->queue.push({_worldPose->Data(), _entityWrench->Data()});
+
+      // debugging
+      // {
+      //   std::stringstream ss;
+      //   ss << "Received EntityWrench for entity ["
+      //     << _entity << "]\n"
+      //     << "WorldPose [" << _worldPose->Data().Pos() << "]\n"
+      //     << "Msg: " << _entityWrench->Data().DebugString() << "\n";
+      //   gzdbg << ss.str();
+      // }
+
+      return true;
+    });
+  }
+#endif
 }
 
 /////////////////////////////////////////////////