diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 5aab1783c8..4ea88aa0be 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/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(); }; } } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 1791d68e9a..9d7ebeb16a 100644 --- a/src/gui/GuiRunner.cc +++ b/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([&]() { diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 84b5276e31..18c0c6d5fd 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -200,6 +200,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// has drained. public: std::unordered_map entityOffMap; + /// \brief Entities whose pose commands have been processed and should be + /// deleted the following iteration. + public: std::unordered_set 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( [&](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(entity); + } + // Slip compliance on Collisions _ecm.Each( [&](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 entitiesWorldCmd; - _ecm.Each( - [&](const Entity &_entity, components::WorldPoseCmd*) -> bool - { - entitiesWorldCmd.push_back(_entity); - return true; - }); - - for (const Entity &entity : entitiesWorldCmd) - { - _ecm.RemoveComponent(entity); - } - // Populate bounding box info // Only compute bounding box if component exists to avoid unnecessary // computations diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index f726349ee4..ac93da86b6 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/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 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 diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index 35084060e3..9506d080ba 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/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;