Merge pull request #653 from ignitionrobotics/chapulina/3/scene_broad…

…caster_paused

Suggestion to #497

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;
 
@@ -74,10 +78,6 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject
 
   /// \brief Latest update info
   private: UpdateInfo updateInfo;
-
-  /// \brief Update the plugins.
-  /// \todo(anyone) Move to a private data class.
-  private: void UpdatePlugins();
 };
 }
 }

src/gui/GuiRunner.cc

@@ -30,6 +30,7 @@
 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;
 
@@ -62,8 +63,7 @@ GuiRunner::GuiRunner(const std::string &_worldName)
   this->RequestState();
 
   // Periodically update the plugins
-  // \todo(anyone) Pimplize GuiRunner and move these global variables to the
-  // private class.
+  // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5
   gRunning = true;
   gUpdateThread = std::thread([&]()
   {

src/systems/physics/Physics.cc

@@ -200,6 +200,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 +1454,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 +1518,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 +1657,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

@@ -198,9 +198,6 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
 
   /// \brief A list of async state requests
   public: std::unordered_set<std::string> stateRequests;
-
-  /// \brief Stores change event information during PreUpdate.
-  public: bool changeEvent{false};
 };
 
 //////////////////////////////////////////////////
@@ -242,16 +239,6 @@ void SceneBroadcaster::Configure(
   }
 }
 
-//////////////////////////////////////////////////
-void SceneBroadcaster::PreUpdate(const UpdateInfo &_info,
-    EntityComponentManager &_ecm)
-{
-  bool jumpBackInTime = _info.dt < std::chrono::steady_clock::duration::zero();
-  this->dataPtr->changeEvent =  _ecm.HasEntitiesMarkedForRemoval() ||
-        _ecm.HasNewEntities() || _ecm.HasOneTimeComponentChanges() ||
-        jumpBackInTime;
-}
-
 //////////////////////////////////////////////////
 void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
     const EntityComponentManager &_manager)
@@ -285,15 +272,14 @@ 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();
-  this->dataPtr->changeEvent =  this->dataPtr->changeEvent ||
-    _manager.HasEntitiesMarkedForRemoval() ||
+  bool changeEvent = _manager.HasEntitiesMarkedForRemoval() ||
     _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() &&
-       (this->dataPtr->changeEvent || itsPubTime);
+       (changeEvent || itsPubTime);
 
   if (this->dataPtr->stateServiceRequest || shouldPublish)
   {
@@ -303,7 +289,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
     set(this->dataPtr->stepMsg.mutable_stats(), _info);
 
     // Publish full state if there are change events
-    if (this->dataPtr->changeEvent || this->dataPtr->stateServiceRequest)
+    if (changeEvent || this->dataPtr->stateServiceRequest)
     {
       _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true);
     }
@@ -932,7 +918,6 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity,
 IGNITION_ADD_PLUGIN(SceneBroadcaster,
                     ignition::gazebo::System,
                     SceneBroadcaster::ISystemConfigure,
-                    SceneBroadcaster::ISystemPreUpdate,
                     SceneBroadcaster::ISystemPostUpdate)
 
 // Add plugin alias so that we can refer to the plugin without the version

src/systems/scene_broadcaster/SceneBroadcaster.hh

@@ -41,7 +41,6 @@ namespace systems
   class IGNITION_GAZEBO_VISIBLE SceneBroadcaster:
     public System,
     public ISystemConfigure,
-    public ISystemPreUpdate,
     public ISystemPostUpdate
   {
     /// \brief Constructor
@@ -56,11 +55,6 @@ namespace systems
                            EntityComponentManager &_ecm,
                            EventManager &_eventMgr) final;
 
-    // Documentation inherited
-    public: void PreUpdate(
-                const ignition::gazebo::UpdateInfo &_info,
-                ignition::gazebo::EntityComponentManager &_ecm) override;
-
     // Documentation inherited
     public: void PostUpdate(const UpdateInfo &_info,
                 const EntityComponentManager &_ecm) final;