VisualizeForces: use msgs::EntityWrenchMap message.

- Use new message type msgs::EntityWrenchMap. - Disable components::EntityWrench and components::EntityWrenches. - Enable components::EntityWrenchMap. Signed-off-by: Rhys Mainwaring <[email protected]>
srmainwaring · Mar 23, 2023 · fd90729 · fd90729
1 parent 53210d2
commit fd90729
Show file tree

Hide file tree

Showing 3 changed files with 113 additions and 2 deletions.
diff --git a/include/gz/sim/components/EntityWrench.hh b/include/gz/sim/components/EntityWrench.hh
@@ -21,6 +21,7 @@
 #include <unordered_map>
 
 #include <gz/msgs/entity_wrench.pb.h>
+#include <gz/msgs/entity_wrench_map.pb.h>
 #include <gz/sim/components/Component.hh>
 #include <gz/sim/components/Factory.hh>
 #include <gz/sim/components/Serialization.hh>
@@ -105,6 +106,14 @@ namespace components
   GZ_SIM_REGISTER_COMPONENT("gz_sim_components.EntityWrench",
                             EntityWrench)
 
+  /// \brief A component type that contains an map of wrenches
+  /// for an entity. The wrenches are represented by gz::msgs::EntityWrenchMap.
+  using EntityWrenchMap =
+      Component<msgs::EntityWrenchMap, class EntityWrenchMapTag,
+          serializers::MsgSerializer>;
+  GZ_SIM_REGISTER_COMPONENT("gz_sim_components.EntityWrenchMap",
+                            EntityWrenchMap)
+
   /// \brief A component type that contains an unordered map of wrenches
   /// for an entity. The wrenches are represented by gz::msgs::EntityWrench.
   /// The key is a string label.

diff --git a/src/Link.cc b/src/Link.cc
@@ -22,6 +22,7 @@
 
 #include <gz/msgs/Utility.hh>
 #include <gz/msgs/entity_wrench.pb.h>
+#include <gz/msgs/entity_wrench_map.pb.h>
 
 #include "gz/sim/components/AngularAcceleration.hh"
 #include "gz/sim/components/AngularVelocity.hh"
@@ -438,7 +439,7 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
               msgs::Convert(linkWrenchComp->Data().torque()) + _torque);
   }
 
-#if 1
+#if 0
   // Publish components::EntityWrenches
   if (this->dataPtr->visualizationLabel.has_value())
   {
@@ -513,6 +514,73 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
 #endif
 
 #if 1
+  // Publish components::EntityWrenchMap
+  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::EntityWrenchMap>(_ecm, this->dataPtr->id, true);
+
+    auto entityWrenchMapComp =
+        _ecm.Component<components::EntityWrenchMap>(this->dataPtr->id);
+    if (!entityWrenchMapComp)
+    {
+      static bool informed{false};
+      if (!informed)
+      {
+        gzerr << "Failed to retrieve EntityWrenchMap 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
+    auto& data = entityWrenchMapComp->Data();
+    (*data.mutable_wrenches())[label] = msg;
+
+    _ecm.SetChanged(this->dataPtr->id, components::EntityWrenchMap::typeId,
+        ComponentState::PeriodicChange);
+
+    // {
+    //   gzdbg << "Publishing entity wrench map for link ["
+    //         << this->dataPtr->id << "]\n"
+    //         << "Size: " << entityWrenchMapComp->Data().wrenches().size() << "\n"
+    //         << "Label: " << label << "\n"
+    //         << entityWrenchMapComp->Data().DebugString() << "\n";
+    // }
+  }
+#endif
+
+#if 0
   /// \todo(srmainwaring) - for debugging - publish components::EntityWrenches
   if (this->dataPtr->visualizationLabel.has_value())
   {

diff --git a/src/gui/plugins/visualize_forces/VisualizeForces.cc b/src/gui/plugins/visualize_forces/VisualizeForces.cc
@@ -36,7 +36,7 @@
 #include <gz/math/Vector3.hh>
 
 #include <gz/msgs/entity_wrench.pb.h>
-#include <gz/msgs/wrench.pb.h>
+#include <gz/msgs/entity_wrench_map.pb.h>
 
 #include <gz/plugin/Register.hh>
 
@@ -165,6 +165,40 @@ void VisualizeForcesPrivate::RetrieveWrenchesFromEcm(
 #endif
 
 #if 1
+  {
+    // Read components::EntityWrenchMap
+    _ecm.Each<components::WorldPose, components::EntityWrenchMap>(
+      [&](const Entity &_entity,
+          components::WorldPose *_worldPose,
+          components::EntityWrenchMap *_entityWrenchMap) -> bool
+      {
+      // Push all the wrenches for this entity to the queue.
+      for (auto& [key, value] : _entityWrenchMap->Data().wrenches())
+      {
+        this->queue.push({_worldPose->Data(), value});
+      }
+
+      // debugging
+      // {
+      //   std::stringstream ss;
+      //   ss << "Received EntityWrenchMap for entity ["
+      //     << _entity << "]\n"
+      //     << "WorldPose [" << _worldPose->Data().Pos() << "]\n"
+      //     << "Size " << _entityWrenchMap->Data().wrenches().size() << "\n";
+      //   for (auto& [key, value] : _entityWrenchMap->Data().wrenches())
+      //   {
+      //     ss << "Key: " << key << "\n"
+      //        << "Msg: " << value.DebugString() << "\n";
+      //   }
+      //   gzdbg << ss.str();
+      // }
+
+      return true;
+    });
+  }
+#endif
+
+#if 0
   {
     /// \todo(srmainwaring) - for debugging - read components::EntityWrench
     _ecm.Each<components::WorldPose, components::EntityWrench>(