Limit scene broadcast publications when paused

include/ignition/gazebo/gui/GuiRunner.hh

@@ -63,6 +63,10 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject
   /// \param[in] _msg New state message.
   private: void OnState(const msgs::SerializedStepMap &_msg);
 
+  /// \brief Update the plugins.
+  /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5
+  private: void UpdatePlugins();
+
   /// \brief Entity-component manager.
   private: gazebo::EntityComponentManager ecm;
 

src/gui/GuiRunner.cc

@@ -30,6 +30,16 @@
 using namespace ignition;
 using namespace gazebo;
 
+/// \todo(anyone) Move to GuiRunner::Implementation when porting to v5
+/// \brief Flag used to end the gUpdateThread.
+static bool gRunning = false;
+
+/// \brief Mutex to protect the plugin update.
+static std::mutex gUpdateMutex;
+
+/// \brief The plugin update thread..
+static std::thread gUpdateThread;
+
 /////////////////////////////////////////////////
 GuiRunner::GuiRunner(const std::string &_worldName)
 {
@@ -51,10 +61,31 @@ GuiRunner::GuiRunner(const std::string &_worldName)
          << std::endl;
 
   this->RequestState();
+
+  // Periodically update the plugins
+  // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5
+  gRunning = true;
+  gUpdateThread = std::thread([&]()
+  {
+    while (gRunning)
+    {
+      {
+        std::lock_guard<std::mutex> lock(gUpdateMutex);
+        this->UpdatePlugins();
+      }
+      // This is roughly a 30Hz update rate.
+      std::this_thread::sleep_for(std::chrono::milliseconds(33));
+    }
+  });
 }
 
 /////////////////////////////////////////////////
-GuiRunner::~GuiRunner() = default;
+GuiRunner::~GuiRunner()
+{
+  gRunning = false;
+  if (gUpdateThread.joinable())
+    gUpdateThread.join();
+}
 
 /////////////////////////////////////////////////
 void GuiRunner::RequestState()
@@ -108,16 +139,22 @@ void GuiRunner::OnState(const msgs::SerializedStepMap &_msg)
   IGN_PROFILE_THREAD_NAME("GuiRunner::OnState");
   IGN_PROFILE("GuiRunner::Update");
 
+  std::lock_guard<std::mutex> lock(gUpdateMutex);
   this->ecm.SetState(_msg.state());
 
   // Update all plugins
   this->updateInfo = convert<UpdateInfo>(_msg.stats());
+  this->UpdatePlugins();
+  this->ecm.ClearNewlyCreatedEntities();
+  this->ecm.ProcessRemoveEntityRequests();
+}
+
+/////////////////////////////////////////////////
+void GuiRunner::UpdatePlugins()
+{
   auto plugins = gui::App()->findChildren<GuiSystem *>();
   for (auto plugin : plugins)
   {
     plugin->Update(this->updateInfo, this->ecm);
   }
-  this->ecm.ClearNewlyCreatedEntities();
-  this->ecm.ProcessRemoveEntityRequests();
 }
-

src/systems/physics/Physics.cc

@@ -27,6 +27,7 @@
 #include <deque>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include <ignition/common/MeshManager.hh>
@@ -200,6 +201,10 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// has drained.
   public: std::unordered_map<Entity, bool> entityOffMap;
 
+  /// \brief Entities whose pose commands have been processed and should be
+  /// deleted the following iteration.
+  public: std::unordered_set<Entity> worldPoseCmdsToRemove;
+
   /// \brief used to store whether physics objects have been created.
   public: bool initialized = false;
 
@@ -1450,10 +1455,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       });
 
   // Update model pose
+  auto olderWorldPoseCmdsToRemove = std::move(this->worldPoseCmdsToRemove);
+  this->worldPoseCmdsToRemove.clear();
+
   _ecm.Each<components::Model, components::WorldPoseCmd>(
       [&](const Entity &_entity, const components::Model *,
           const components::WorldPoseCmd *_poseCmd)
       {
+        this->worldPoseCmdsToRemove.insert(_entity);
+
         auto modelIt = this->entityModelMap.find(_entity);
         if (modelIt == this->entityModelMap.end())
           return true;
@@ -1509,6 +1519,13 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Remove world commands from previous iteration. We let them rotate one
+  // iteration so other systems have a chance to react to them too.
+  for (const Entity &entity : olderWorldPoseCmdsToRemove)
+  {
+    _ecm.RemoveComponent<components::WorldPoseCmd>(entity);
+  }
+
   // Slip compliance on Collisions
   _ecm.Each<components::SlipComplianceCmd>(
       [&](const Entity &_entity,
@@ -1641,23 +1658,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
-  // Clear pending commands
-  // Note: Removing components from inside an Each call can be dangerous.
-  // Instead, we collect all the entities that have the desired components and
-  // remove the component from them afterward.
-  std::vector<Entity> entitiesWorldCmd;
-  _ecm.Each<components::WorldPoseCmd>(
-      [&](const Entity &_entity, components::WorldPoseCmd*) -> bool
-      {
-        entitiesWorldCmd.push_back(_entity);
-        return true;
-      });
-
-  for (const Entity &entity : entitiesWorldCmd)
-  {
-    _ecm.RemoveComponent<components::WorldPoseCmd>(entity);
-  }
-
   // Populate bounding box info
   // Only compute bounding box if component exists to avoid unnecessary
   // computations

src/systems/scene_broadcaster/SceneBroadcaster.cc

@@ -272,12 +272,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
   // Throttle here instead of using transport::AdvertiseMessageOptions so that
   // we can skip the ECM serialization
   bool jumpBackInTime = _info.dt < std::chrono::steady_clock::duration::zero();
-  auto now = std::chrono::system_clock::now();
   bool changeEvent = _manager.HasEntitiesMarkedForRemoval() ||
-        _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() ||
-        jumpBackInTime;
-  bool itsPubTime = now - this->dataPtr->lastStatePubTime >
-       this->dataPtr->statePublishPeriod;
+    _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() ||
+    jumpBackInTime;
+  auto now = std::chrono::system_clock::now();
+  bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime >
+       this->dataPtr->statePublishPeriod);
   auto shouldPublish = this->dataPtr->statePub.HasConnections() &&
        (changeEvent || itsPubTime);
 

src/systems/scene_broadcaster/SceneBroadcaster.hh

@@ -55,6 +55,7 @@ namespace systems
                            EntityComponentManager &_ecm,
                            EventManager &_eventMgr) final;
 
+    // Documentation inherited
     public: void PostUpdate(const UpdateInfo &_info,
                 const EntityComponentManager &_ecm) final;