Skip to content

Commit

Permalink
Use parentEntity available in systems instead of storing it for eac…
Browse files Browse the repository at this point in the history 
…h interface (#2473)


---------

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjoc@google.com>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Arjo Chakravarty <arjoc@intrinsic.ai>
  • Loading branch information
@azeey @scpeters @arjo129
3 people committed Jul 12, 2024
1 parent 00a6e47 commit 7026b57
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 105 deletions.
9 changes: 3 additions & 6 deletions src/SimulationRunner.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -571,12 +571,10 @@ void SimulationRunner::ProcessSystemQueue()
this->postUpdateStartBarrier->Wait();
if (this->postUpdateThreadsRunning)
{
system->PostUpdate(this->currentInfo,
this->entityCompMgr);
system->PostUpdate(this->currentInfo, this->entityCompMgr);
}
this->postUpdateStopBarrier->Wait();
}

gzdbg << "Exiting postupdate worker thread ("
<< id << ")" << std::endl;
}));
Expand Down Expand Up @@ -604,13 +602,13 @@ void SimulationRunner::UpdateSystems()
{
GZ_PROFILE("PreUpdate");
for (auto& system : this->systemMgr->SystemsPreUpdate())
system.system->PreUpdate(this->currentInfo, this->entityCompMgr);
system->PreUpdate(this->currentInfo, this->entityCompMgr);
}

{
GZ_PROFILE("Update");
for (auto& system : this->systemMgr->SystemsUpdate())
system.system->Update(this->currentInfo, this->entityCompMgr);
system->Update(this->currentInfo, this->entityCompMgr);
}

{
Expand Down Expand Up @@ -895,7 +893,6 @@ void SimulationRunner::Step(const UpdateInfo &_info)
// Update all the systems.
this->UpdateSystems();


if (!this->Paused() && this->requestedRunToSimTime &&
this->requestedRunToSimTime.value() > this->simTimeEpoch &&
this->currentInfo.simTime >= this->requestedRunToSimTime.value())
Expand Down
145 changes: 86 additions & 59 deletions src/SystemManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,31 +110,23 @@ size_t SystemManager::ActivatePendingSystems()
this->systems.push_back(system);

if (system.configure)
this->systemsConfigure.emplace_back(
system.parentEntity,
system.configure);
this->systemsConfigure.push_back(system.configure);

if (system.configureParameters)
this->systemsConfigureParameters.emplace_back(
system.parentEntity,
system.configureParameters);
this->systemsConfigureParameters.push_back(system.configureParameters);

if (system.reset)
this->systemsReset.emplace_back(system.parentEntity,
system.reset);
this->systemsReset.push_back(system.reset);

if (system.preupdate)
this->systemsPreupdate.emplace_back(system.parentEntity,
system.preupdate);
this->systemsPreupdate.push_back(system.preupdate);

if (system.update)
this->systemsUpdate.emplace_back(system.parentEntity,
system.update);
this->systemsUpdate.push_back(system.update);

if (system.postupdate)
{
this->systemsPostupdate.push_back(system.postupdate);
this->postUpdateParents.push_back(system.parentEntity);
}
}

Expand Down Expand Up @@ -169,7 +161,6 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm)
this->systemsPreupdate.clear();
this->systemsUpdate.clear();
this->systemsPostupdate.clear();
this->postUpdateParents.clear();

std::vector<PluginInfo> pluginsToBeLoaded;

Expand Down Expand Up @@ -291,42 +282,37 @@ void SystemManager::AddSystemImpl(
}

//////////////////////////////////////////////////
const std::vector<SystemIfaceWithParent<ISystemConfigure>>&
SystemManager::SystemsConfigure()
const std::vector<ISystemConfigure *>& SystemManager::SystemsConfigure()
{
return this->systemsConfigure;
}

const std::vector<SystemIfaceWithParent<ISystemConfigureParameters>>&
const std::vector<ISystemConfigureParameters *>&
SystemManager::SystemsConfigureParameters()
{
return this->systemsConfigureParameters;
}

//////////////////////////////////////////////////
const std::vector<SystemIfaceWithParent<ISystemReset>>&
SystemManager::SystemsReset()
const std::vector<ISystemReset *> &SystemManager::SystemsReset()
{
return this->systemsReset;
}

//////////////////////////////////////////////////
const std::vector<SystemIfaceWithParent<ISystemPreUpdate>>&
SystemManager::SystemsPreUpdate()
const std::vector<ISystemPreUpdate *>& SystemManager::SystemsPreUpdate()
{
return this->systemsPreupdate;
}

//////////////////////////////////////////////////
const std::vector<SystemIfaceWithParent<ISystemUpdate>>&
SystemManager::SystemsUpdate()
const std::vector<ISystemUpdate *>& SystemManager::SystemsUpdate()
{
return this->systemsUpdate;
}

//////////////////////////////////////////////////
const std::vector<ISystemPostUpdate *>&
SystemManager::SystemsPostUpdate()
const std::vector<ISystemPostUpdate *>& SystemManager::SystemsPostUpdate()
{
return this->systemsPostupdate;
}
Expand Down Expand Up @@ -460,58 +446,99 @@ void SystemManager::ProcessRemovedEntities(
return;
}

std::unordered_set<ISystemReset *> resetSystemsToBeRemoved;
std::unordered_set<ISystemPreUpdate *> preupdateSystemsToBeRemoved;
std::unordered_set<ISystemUpdate *> updateSystemsToBeRemoved;
std::unordered_set<ISystemPostUpdate *> postupdateSystemsToBeRemoved;
std::unordered_set<ISystemConfigure *> configureSystemsToBeRemoved;
std::unordered_set<ISystemConfigureParameters *>
configureParametersSystemsToBeRemoved;
for (const auto &system : this->systems)
{
if (_ecm.IsMarkedForRemoval(system.parentEntity))
{
if (system.reset)
{
resetSystemsToBeRemoved.insert(system.reset);
}
if (system.preupdate)
{
preupdateSystemsToBeRemoved.insert(system.preupdate);
}
if (system.update)
{
updateSystemsToBeRemoved.insert(system.update);
}
if (system.postupdate)
{
postupdateSystemsToBeRemoved.insert(system.postupdate);
// If system with a PostUpdate is marked for removal
// mark all worker threads for removal.
_needsCleanUp = true;
}
if (system.configure)
{
configureSystemsToBeRemoved.insert(system.configure);
}
if (system.configureParameters)
{
configureParametersSystemsToBeRemoved.insert(
system.configureParameters);
}
}
}

RemoveFromVectorIf(this->systemsReset,
[&](const SystemIfaceWithParent<ISystemReset>& system) {
return _ecm.IsMarkedForRemoval(system.parent);
[&](const auto system) {
if (resetSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});
RemoveFromVectorIf(this->systemsPreupdate,
[&](const SystemIfaceWithParent<ISystemPreUpdate>& system) {
return _ecm.IsMarkedForRemoval(system.parent);
[&](const auto& system) {
if (preupdateSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});
RemoveFromVectorIf(this->systemsUpdate,
[&](const SystemIfaceWithParent<ISystemUpdate>& system) {
return _ecm.IsMarkedForRemoval(system.parent);
[&](const auto& system) {
if (updateSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});

std::unordered_set<ISystemPostUpdate *> markedForRemoval;
for (std::size_t i = 0; i < this->systemsPostupdate.size(); i++)
{
if(_ecm.IsMarkedForRemoval(postUpdateParents[i]))
{
// If system with a PostUpdate is marked for removal
// mark all worker threads for removal.
_needsCleanUp = true;
markedForRemoval.insert(this->systemsPostupdate[i]);
}
}

RemoveFromVectorIf(this->systemsPostupdate,
[&](const auto system) {
if (markedForRemoval.count(system)) {
[&](const auto& system) {
if (postupdateSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});

RemoveFromVectorIf(this->postUpdateParents,
[&](const Entity& entity){
return _ecm.IsMarkedForRemoval(entity);
}
);
RemoveFromVectorIf(this->systemsConfigure,
[&](const SystemIfaceWithParent<ISystemConfigure>& system) {
return _ecm.IsMarkedForRemoval(system.parent);
[&](const auto& system) {
if (configureSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});
RemoveFromVectorIf(this->systemsConfigureParameters,
[&](const SystemIfaceWithParent<ISystemConfigureParameters>& system) {
return _ecm.IsMarkedForRemoval(system.parent);
[&](const auto& system) {
if (configureParametersSystemsToBeRemoved.count(system)) {
return true;
}
return false;
});
RemoveFromVectorIf(this->systems, [&](const SystemInternal& system) {
return _ecm.IsMarkedForRemoval(system.parentEntity);
RemoveFromVectorIf(this->systems,
[&](const SystemInternal& _arg) {
return _ecm.IsMarkedForRemoval(_arg.parentEntity);
});

std::lock_guard lock(this->pendingSystemsMutex);
RemoveFromVectorIf(this->pendingSystems,
[&](const SystemInternal& system) {
return _ecm.IsMarkedForRemoval(system.parentEntity);
[&](const SystemInternal& _system) {
return _ecm.IsMarkedForRemoval(_system.parentEntity);
});
}
52 changes: 12 additions & 40 deletions src/SystemManager.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@

#include <memory>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <sdf/Plugin.hh>
Expand All @@ -43,21 +41,6 @@ namespace gz
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE {

/// \brief Helper container to keep track of
/// system interfaces and their parents
template<typename IFace>
struct SystemIfaceWithParent {
/// Parent entity of system
Entity parent;

/// Interface pointer
IFace* system;

/// \brief constructor
SystemIfaceWithParent(Entity _parent, IFace* _iface):
parent(_parent), system(_iface) {}
};

/// \brief Used to load / unload sysetms as well as iterate over them.
class GZ_SIM_VISIBLE SystemManager
{
Expand Down Expand Up @@ -131,35 +114,29 @@ namespace gz

/// \brief Get a vector of all systems implementing "Configure"
/// \return Vector of systems' configure interfaces.
public: const std::vector<SystemIfaceWithParent<ISystemConfigure>>&
SystemsConfigure();
public: const std::vector<ISystemConfigure *>& SystemsConfigure();

/// \brief Get an vector of all active systems implementing
/// "ConfigureParameters"
/// \return Vector of systems's configure interfaces.
public: const std::vector<
SystemIfaceWithParent<ISystemConfigureParameters>>&
SystemsConfigureParameters();
public: const std::vector<ISystemConfigureParameters *>&
SystemsConfigureParameters();

/// \brief Get an vector of all active systems implementing "Reset"
/// \return Vector of systems' reset interfaces.
public: const std::vector<SystemIfaceWithParent<ISystemReset>>&
SystemsReset();
public: const std::vector<ISystemReset *>& SystemsReset();

/// \brief Get an vector of all active systems implementing "PreUpdate"
/// \return Vector of systems's pre-update interfaces.
public: const std::vector<SystemIfaceWithParent<ISystemPreUpdate>>&
SystemsPreUpdate();
public: const std::vector<ISystemPreUpdate *>& SystemsPreUpdate();

/// \brief Get an vector of all active systems implementing "Update"
/// \return Vector of systems's update interfaces.
public: const std::vector<SystemIfaceWithParent<ISystemUpdate>>&
SystemsUpdate();
public: const std::vector<ISystemUpdate *>& SystemsUpdate();

/// \brief Get an vector of all active systems implementing "PostUpdate"
/// \return Vector of systems's post-update interfaces.
public: const std::vector<ISystemPostUpdate *>&
SystemsPostUpdate();
public: const std::vector<ISystemPostUpdate *>& SystemsPostUpdate();

/// \brief Get an vector of all systems attached to a given entity.
/// \return Vector of systems.
Expand Down Expand Up @@ -218,29 +195,24 @@ namespace gz
private: mutable std::mutex pendingSystemsMutex;

/// \brief Systems implementing Configure
private: std::vector<SystemIfaceWithParent<ISystemConfigure>>
systemsConfigure;
private: std::vector<ISystemConfigure *> systemsConfigure;

/// \brief Systems implementing ConfigureParameters
private: std::vector<SystemIfaceWithParent<ISystemConfigureParameters>>
private: std::vector<ISystemConfigureParameters *>
systemsConfigureParameters;

/// \brief Systems implementing Reset
private: std::vector<SystemIfaceWithParent<ISystemReset>> systemsReset;
private: std::vector<ISystemReset *> systemsReset;

/// \brief Systems implementing PreUpdate
private: std::vector<SystemIfaceWithParent<ISystemPreUpdate>>
systemsPreupdate;
private: std::vector<ISystemPreUpdate *> systemsPreupdate;

/// \brief Systems implementing Update
private: std::vector<SystemIfaceWithParent<ISystemUpdate>> systemsUpdate;
private: std::vector<ISystemUpdate *> systemsUpdate;

/// \brief Systems implementing PostUpdate
private: std::vector<ISystemPostUpdate *> systemsPostupdate;

/// \brief Parents of post update systems.
private: std::vector<Entity> postUpdateParents;

/// \brief System loader, for loading system plugins.
private: SystemLoaderPtr systemLoader;

Expand Down

0 comments on commit 7026b57

Please sign in to comment.