From e1a23c4b0651ec3e9f043e285341003e3740663c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 17 Dec 2020 14:20:35 -0800 Subject: [PATCH 1/6] Limit scene broadcast publications when paused Signed-off-by: Nate Koenig --- src/systems/scene_broadcaster/SceneBroadcaster.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 576a081bc0..a9c2df14f0 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -271,8 +271,8 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, bool changeEvent = _manager.HasEntitiesMarkedForRemoval() || _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() || jumpBackInTime; - bool itsPubTime = now - this->dataPtr->lastStatePubTime > - this->dataPtr->statePublishPeriod; + bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime > + this->dataPtr->statePublishPeriod); auto shouldPublish = this->dataPtr->statePub.HasConnections() && (changeEvent || itsPubTime); From 79c6a4392edb43dd77e549c94346ee31d945cb55 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 22 Dec 2020 08:44:29 -0800 Subject: [PATCH 2/6] Capture changeevent during PreUpdate Signed-off-by: Nate Koenig --- .../scene_broadcaster/SceneBroadcaster.cc | 22 ++++++++++++++----- .../scene_broadcaster/SceneBroadcaster.hh | 7 ++++++ 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index a9c2df14f0..3a1fe7cf07 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -198,6 +198,9 @@ 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}; }; ////////////////////////////////////////////////// @@ -234,6 +237,16 @@ 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) @@ -266,15 +279,11 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // * jump back in time // 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 = !_info.paused && (now - this->dataPtr->lastStatePubTime > this->dataPtr->statePublishPeriod); auto shouldPublish = this->dataPtr->statePub.HasConnections() && - (changeEvent || itsPubTime); + (this->dataPtr->changeEvent || itsPubTime); if (this->dataPtr->stateServiceRequest || shouldPublish) { @@ -284,7 +293,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, set(this->dataPtr->stepMsg.mutable_stats(), _info); // Publish full state if there are change events - if (changeEvent || this->dataPtr->stateServiceRequest) + if (this->dataPtr->changeEvent || this->dataPtr->stateServiceRequest) { _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); } @@ -912,6 +921,7 @@ 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 0f19e23fe0..35084060e3 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -41,6 +41,7 @@ namespace systems class IGNITION_GAZEBO_VISIBLE SceneBroadcaster: public System, public ISystemConfigure, + public ISystemPreUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -55,6 +56,12 @@ 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; From 17bb8f178c5cb97e8c741c5a4cdd10efda90b35a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 23 Dec 2020 12:52:49 -0800 Subject: [PATCH 3/6] Improved scene broadcaster update trigger and added a thread to the GuiRunner that will periodically update the GUI plugins Signed-off-by: Nate Koenig --- include/ignition/gazebo/gui/GuiRunner.hh | 4 ++ src/gui/GuiRunner.cc | 45 +++++++++++++++++-- .../scene_broadcaster/SceneBroadcaster.cc | 5 +++ 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 43a33a795e..5aab1783c8 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -74,6 +74,10 @@ 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 88c7f4d46a..1791d68e9a 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -30,6 +30,15 @@ using namespace ignition; using namespace gazebo; +/// \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 +60,32 @@ GuiRunner::GuiRunner(const std::string &_worldName) << std::endl; this->RequestState(); + + // Periodically update the plugins + // \todo(anyone) Pimplize GuiRunner and move these global variables to the + // private class. + gRunning = true; + gUpdateThread = std::thread([&]() + { + while (gRunning) + { + { + std::lock_guard 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 lock(gUpdateMutex); this->ecm.SetState(_msg.state()); // Update all plugins this->updateInfo = convert(_msg.stats()); + this->UpdatePlugins(); + this->ecm.ClearNewlyCreatedEntities(); + this->ecm.ProcessRemoveEntityRequests(); +} + +///////////////////////////////////////////////// +void GuiRunner::UpdatePlugins() +{ auto plugins = gui::App()->findChildren(); for (auto plugin : plugins) { plugin->Update(this->updateInfo, this->ecm); } - this->ecm.ClearNewlyCreatedEntities(); - this->ecm.ProcessRemoveEntityRequests(); } - diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 3a1fe7cf07..44cf8d6c44 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -279,6 +279,11 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // * jump back in time // 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() || + _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() || + jumpBackInTime; auto now = std::chrono::system_clock::now(); bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime > this->dataPtr->statePublishPeriod); From abcf35fe6edfbdbe4ec987e9d841ef2caebf69cf Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 25 Feb 2021 14:47:05 -0800 Subject: [PATCH 4/6] Suggestion to #497 Signed-off-by: Louise Poubel --- include/ignition/gazebo/gui/GuiRunner.hh | 8 ++--- src/gui/GuiRunner.cc | 4 +-- src/systems/physics/Physics.cc | 33 +++++++++---------- .../scene_broadcaster/SceneBroadcaster.cc | 21 ++---------- .../scene_broadcaster/SceneBroadcaster.hh | 6 ---- 5 files changed, 25 insertions(+), 47 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 5aab1783c8..3e7eafd371 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 GuiRunnerPrivate 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..8b70f6ceda 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 GuiRunnerPrivate 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 these global variables to GuiRunnerPrivate 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; From 3813ba5db67d260b4c70af4e8feb622a705f1df9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 25 Feb 2021 14:51:18 -0800 Subject: [PATCH 5/6] Implementation Signed-off-by: Louise Poubel --- include/ignition/gazebo/gui/GuiRunner.hh | 2 +- src/gui/GuiRunner.cc | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 3e7eafd371..4ea88aa0be 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -64,7 +64,7 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject private: void OnState(const msgs::SerializedStepMap &_msg); /// \brief Update the plugins. - /// \todo(anyone) Move to GuiRunnerPrivate when porting to v5 + /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 private: void UpdatePlugins(); /// \brief Entity-component manager. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 8b70f6ceda..9d7ebeb16a 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -30,7 +30,7 @@ using namespace ignition; using namespace gazebo; -/// \todo(anyone) Move to GuiRunnerPrivate when porting to v5 +/// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 /// \brief Flag used to end the gUpdateThread. static bool gRunning = false; @@ -63,7 +63,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) this->RequestState(); // Periodically update the plugins - // \todo(anyone) Move these global variables to GuiRunnerPrivate on v5 + // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5 gRunning = true; gUpdateThread = std::thread([&]() { From 9506604930273c20684248d2ff669fe9d8a4b803 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 25 Feb 2021 17:02:27 -0800 Subject: [PATCH 6/6] codecheck Signed-off-by: Louise Poubel --- src/systems/physics/Physics.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 18c0c6d5fd..86ee7162a7 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -27,6 +27,7 @@ #include #include #include +#include #include #include