Distributed: Add debug info to NetworkManagerSecondary

- Add error message if secondary fails to remove parent entity.
- Only remove models if performer is assigned to another secondary.
- Set the secondary namespace on the update info.
- Add debug output.

Signed-off-by: Rhys Mainwaring <[email protected]>

Author: srmainwaring

src/network/NetworkManagerSecondary.cc

@@ -27,6 +27,7 @@
 #include <gz/common/Profiler.hh>
 
 #include "gz/sim/components/ParentEntity.hh"
+#include "gz/sim/components/PerformerAffinity.hh"
 #include "gz/sim/Conversions.hh"
 #include "gz/sim/Entity.hh"
 #include "gz/sim/EntityComponentManager.hh"
@@ -95,6 +96,10 @@ bool NetworkManagerSecondary::OnControl(const private_msgs::PeerControl &_req,
 {
   this->enableSim = _req.enable_sim();
   _resp.set_enable_sim(this->enableSim);
+
+  gzdbg << "Received msg PeerControl: enable_sim: "
+        << this->enableSim << std::endl;
+
   return true;
 }
 
@@ -104,74 +109,130 @@ void NetworkManagerSecondary::OnStep(
 {
   GZ_PROFILE("NetworkManagerSecondary::OnStep");
 
+  // Do not print debug message if paused.
+  if (!_msg.stats().paused())
+  {
+    gzdbg << "NetworkManagerSecondary::OnStep [" << this->Namespace() << "]"
+          << std::endl;
+    if (_msg.affinity_size() > 0)
+    {
+      gzdbg << "Secondary: SimulationStep:\n"
+            << _msg.DebugString() << std::endl;
+    }
+  }
+
   // Throttle the number of step messages going to the debug output.
   if (!_msg.stats().paused() && _msg.stats().iterations() % 1000 == 0)
   {
     gzdbg << "Network iterations: " << _msg.stats().iterations()
-           << std::endl;
+          << std::endl;
   }
 
   // Update affinities
   for (int i = 0; i < _msg.affinity_size(); ++i)
   {
     const auto &affinityMsg = _msg.affinity(i);
-    const auto &entityId = affinityMsg.entity().id();
+    const auto &perfEntity = affinityMsg.entity().id();
 
     if (affinityMsg.secondary_prefix() == this->Namespace())
     {
-      this->performers.insert(entityId);
+      this->performers.insert(perfEntity);
+
+      // Set performer affinity to this secondary
+      this->dataPtr->ecm->RemoveComponent<components::PerformerAffinity>(
+          perfEntity);
+      auto newAffinity = components::PerformerAffinity(this->Namespace());
+      this->dataPtr->ecm->CreateComponent(perfEntity, newAffinity);
 
       gzmsg << "Secondary [" << this->Namespace()
-             << "] assigned affinity to performer [" << entityId << "]."
+             << "] assigned affinity to performer ["
+             << perfEntity << "]."
              << std::endl;
     }
-    // If performer has been assigned to another secondary, remove it
     else
     {
-      auto parent =
-          this->dataPtr->ecm->Component<components::ParentEntity>(entityId);
-      if (parent != nullptr)
-      {
-        this->dataPtr->ecm->RequestRemoveEntity(parent->Data());
-      }
-
-      if (this->performers.find(entityId) != this->performers.end())
+      // If performer assigned to this secondary is also assigned to another
+      // secondary, remove it from performers and the parent model from the ECM.
+      if (this->performers.find(perfEntity) != this->performers.end())
       {
         gzmsg << "Secondary [" << this->Namespace()
-               << "] unassigned affinity to performer [" << entityId << "]."
+               << "] unassigned affinity to performer ["
+               << perfEntity << "]."
                << std::endl;
-        this->performers.erase(entityId);
+        this->performers.erase(perfEntity);
+
+        // Remove performer affinity from this this secondary
+        this->dataPtr->ecm->RemoveComponent<components::PerformerAffinity>(
+            perfEntity);
+
+        auto parent =
+            this->dataPtr->ecm->Component<components::ParentEntity>(perfEntity);
+        if (parent != nullptr)
+        {
+          this->dataPtr->ecm->RequestRemoveEntity(parent->Data());
+          gzmsg << "Secondary [" << this->Namespace()
+                << "] request remove model entity ["
+                << parent->Data() << "]."
+                << std::endl;
+        }
+        else
+        {
+          gzerr << "Secondary [" << this->Namespace()
+                << "] should remove parent for performer ["
+                << perfEntity << "]."
+                << std::endl;
+        }
       }
     }
   }
 
   // Update info
   auto info = convert<UpdateInfo>(_msg.stats());
 
+  //! @todo must set the secondaryNamespace here...
+  info.secondaryNamespace = this->Namespace();
+
   // Step runner
   this->dataPtr->stepFunction(info);
 
   // Update state with all the performer's entities
   std::unordered_set<Entity> entities;
-  for (const auto &perf : this->performers)
+  if (!_msg.stats().paused())
+  {
+    gzdbg << "Secondary: performer: size: "
+          << this->performers.size() << std::endl;
+  }
+  for (const auto &perfEntity : this->performers)
   {
     // Performer model
-    auto parent = this->dataPtr->ecm->Component<components::ParentEntity>(perf);
+    auto parent =
+        this->dataPtr->ecm->Component<components::ParentEntity>(perfEntity);
     if (parent == nullptr)
     {
-      gzerr << "Failed to get parent for performer [" << perf << "]"
-             << std::endl;
+      gzerr << "Secondary: failed to get parent for performer ["
+            << perfEntity << "]"
+            << std::endl;
       continue;
     }
-    auto modelEntity = parent->Data();
 
+    //! @todo why was the performers model entity not also included?
+    auto modelEntity = parent->Data();
+    entities.insert(modelEntity);
+    if (!_msg.stats().paused())
+    {
+      gzdbg << "Secondary: performer [" << perfEntity << "], "
+            << "parent: [" << modelEntity << "]"
+            <<  std:: endl;
+    }
     auto children = this->dataPtr->ecm->Descendants(modelEntity);
     entities.insert(children.begin(), children.end());
   }
 
   msgs::SerializedStateMap stateMsg;
   if (!entities.empty())
+  {
     this->dataPtr->ecm->State(stateMsg, entities);
+  }
   stateMsg.set_has_one_time_component_changes(
     this->dataPtr->ecm->HasOneTimeComponentChanges());