Skip to content

Commit

Permalink
Address reviewer feedback
Browse files Browse the repository at this point in the history
Signed-off-by: Michael Carroll <michael@openrobotics.org>
  • Loading branch information
mjcarroll committed Feb 21, 2022
1 parent 01300a0 commit cd5552e
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 26 deletions.
12 changes: 4 additions & 8 deletions src/SimulationRunner.cc
Original file line number Diff line number Diff line change
Expand Up @@ -456,10 +456,8 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system,
std::optional<Entity> _entity,
std::optional<std::shared_ptr<const sdf::Element>> _sdf)
{
auto entity = _entity.has_value() ? _entity.value()
: worldEntity(this->entityCompMgr);
auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();

auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
auto sdf = _sdf.value_or(this->sdfWorld->Element());
this->systemMgr->AddSystem(_system, entity, sdf);
}

Expand All @@ -469,10 +467,8 @@ void SimulationRunner::AddSystem(
std::optional<Entity> _entity,
std::optional<std::shared_ptr<const sdf::Element>> _sdf)
{
auto entity = _entity.has_value() ? _entity.value()
: worldEntity(this->entityCompMgr);
auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();

auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
auto sdf = _sdf.value_or(this->sdfWorld->Element());
this->systemMgr->AddSystem(_system, entity, sdf);
}

Expand Down
10 changes: 4 additions & 6 deletions src/SystemManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -54,28 +54,26 @@ void SystemManager::LoadPlugin(const Entity _entity,
//////////////////////////////////////////////////
size_t SystemManager::TotalCount() const
{
std::lock_guard<std::mutex> lock(this->systemsMutex);
return this->systems.size() + this->pendingSystems.size();
return this->ActiveCount() + this->PendingCount();
}

//////////////////////////////////////////////////
size_t SystemManager::ActiveCount() const
{
std::lock_guard<std::mutex> lock(this->systemsMutex);
return this->systems.size();
}

//////////////////////////////////////////////////
size_t SystemManager::PendingCount() const
{
std::lock_guard<std::mutex> lock(this->systemsMutex);
std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
return this->pendingSystems.size();
}

//////////////////////////////////////////////////
size_t SystemManager::ActivatePendingSystems()
{
std::lock_guard<std::mutex> lock(this->systemsMutex);
std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);

auto count = this->pendingSystems.size();

Expand Down Expand Up @@ -132,7 +130,7 @@ void SystemManager::AddSystemImpl(
}

// Update callbacks will be handled later, add to queue
std::lock_guard<std::mutex> lock(this->systemsMutex);
std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
this->pendingSystems.push_back(_system);
}

Expand Down
5 changes: 1 addition & 4 deletions src/SystemManager.hh
Original file line number Diff line number Diff line change
Expand Up @@ -120,11 +120,8 @@ namespace ignition
/// \brief Pending systems to be added to systems.
private: std::vector<SystemInternal> pendingSystems;

/// \brief Mark if a pending system has been configured
private: std::vector<bool> pendingSystemsConfigured;

/// \brief Mutex to protect pendingSystems
private: mutable std::mutex systemsMutex;
private: mutable std::mutex pendingSystemsMutex;

/// \brief Systems implementing Configure
private: std::vector<ISystemConfigure *> systemsConfigure;
Expand Down
16 changes: 8 additions & 8 deletions src/SystemManager_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
using namespace ignition::gazebo;

/////////////////////////////////////////////////
class System_WithConfigure:
class SystemWithConfigure:
public System,
public ISystemConfigure
{
Expand All @@ -43,7 +43,7 @@ class System_WithConfigure:
};

/////////////////////////////////////////////////
class System_WithUpdates:
class SystemWithUpdates:
public System,
public ISystemPreUpdate,
public ISystemUpdate,
Expand Down Expand Up @@ -79,7 +79,7 @@ TEST(SystemManager, Constructor)
}

/////////////////////////////////////////////////
TEST(SystemManager, AddSystem_NoEcm)
TEST(SystemManager, AddSystemNoEcm)
{
auto loader = std::make_shared<SystemLoader>();
SystemManager systemMgr(loader);
Expand All @@ -92,7 +92,7 @@ TEST(SystemManager, AddSystem_NoEcm)
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());

auto configSystem = std::make_shared<System_WithConfigure>();
auto configSystem = std::make_shared<SystemWithConfigure>();
systemMgr.AddSystem(configSystem, kNullEntity, nullptr);

// SystemManager without an ECM/EventmManager will mean no config occurs
Expand All @@ -115,7 +115,7 @@ TEST(SystemManager, AddSystem_NoEcm)
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());

auto updateSystem = std::make_shared<System_WithUpdates>();
auto updateSystem = std::make_shared<SystemWithUpdates>();
systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
EXPECT_EQ(1u, systemMgr.ActiveCount());
EXPECT_EQ(1u, systemMgr.PendingCount());
Expand All @@ -136,7 +136,7 @@ TEST(SystemManager, AddSystem_NoEcm)
}

/////////////////////////////////////////////////
TEST(SystemManager, AddSystem_Ecm)
TEST(SystemManager, AddSystemEcm)
{
auto loader = std::make_shared<SystemLoader>();

Expand All @@ -153,7 +153,7 @@ TEST(SystemManager, AddSystem_Ecm)
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());

auto configSystem = std::make_shared<System_WithConfigure>();
auto configSystem = std::make_shared<SystemWithConfigure>();
systemMgr.AddSystem(configSystem, kNullEntity, nullptr);

// Configure called during AddSystem
Expand All @@ -176,7 +176,7 @@ TEST(SystemManager, AddSystem_Ecm)
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());

auto updateSystem = std::make_shared<System_WithUpdates>();
auto updateSystem = std::make_shared<SystemWithUpdates>();
systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
EXPECT_EQ(1u, systemMgr.ActiveCount());
EXPECT_EQ(1u, systemMgr.PendingCount());
Expand Down
5 changes: 5 additions & 0 deletions src/WorldControl.hh
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,11 @@
#ifndef IGNITION_GAZEBO_WORLDCONTROL_HH_
#define IGNITION_GAZEBO_WORLDCONTROL_HH_

#include <chrono>
#include <cstdint>

#include "ignition/gazebo/config.hh"

namespace ignition
{
namespace gazebo
Expand Down

0 comments on commit cd5552e

Please sign in to comment.