From d8c604774626c1f3472b0c0c79c1f4195de3bcf8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 27 Jul 2021 17:42:14 -0700 Subject: [PATCH] PR feedback Signed-off-by: Louise Poubel --- include/ignition/gazebo/Server.hh | 10 +-- src/Server.cc | 2 +- src/Server_TEST.cc | 10 +-- src/SimulationRunner.cc | 85 +++++++++---------- src/SimulationRunner.hh | 27 +++--- test/helpers/Relay.hh | 19 ++--- test/integration/level_manager.cc | 2 +- .../level_manager_runtime_performers.cc | 2 +- 8 files changed, 79 insertions(+), 78 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 6023934761..fb2e7a17ef 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -220,13 +220,13 @@ namespace ignition const unsigned int _worldIndex = 0); /// \brief Add a System to the server. The server must not be running when - /// calling this. The server does not take ownership of the pointer and - /// assumes that the pointer is valid during the server's lifetime. - /// \param[in] _system system to be added - /// \param[in] _worldIndex Index of the world to query. + /// calling this. + /// \param[in] _system System to be added + /// \param[in] _worldIndex Index of the world to add to. /// \return Whether the system was added successfully, or std::nullopt /// if _worldIndex is invalid. - public: std::optional AddSystem(System *_system, + public: std::optional AddSystem( + const std::shared_ptr &_system, const unsigned int _worldIndex = 0); /// \brief Get an Entity based on a name. diff --git a/src/Server.cc b/src/Server.cc index cd4938954d..7e7de3b975 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -355,7 +355,7 @@ std::optional Server::AddSystem(const SystemPluginPtr &_system, } ////////////////////////////////////////////////// -std::optional Server::AddSystem(System *_system, +std::optional Server::AddSystem(const std::shared_ptr &_system, const unsigned int _worldIndex) { std::lock_guard lock(this->dataPtr->runMutex); diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 43793cab52..89e8c90387 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -733,8 +733,8 @@ TEST_P(ServerFixture, AddSystemWhileRunning) EXPECT_EQ(3u, *server.SystemCount()); // Add system pointer - auto mockSystem = std::make_unique(); - result = server.AddSystem(mockSystem.get()); + auto mockSystem = std::make_shared(); + result = server.AddSystem(mockSystem); ASSERT_TRUE(result.has_value()); EXPECT_FALSE(result.value()); EXPECT_EQ(3u, *server.SystemCount()); @@ -778,10 +778,10 @@ TEST_P(ServerFixture, AddSystemAfterLoad) EXPECT_EQ(1u, mockSystem->configureCallCount); // Add system pointer - auto mockSystemLocal = std::make_unique(); + auto mockSystemLocal = std::make_shared(); EXPECT_EQ(0u, mockSystemLocal->configureCallCount); - EXPECT_TRUE(server.AddSystem(mockSystemLocal.get())); + EXPECT_TRUE(server.AddSystem(mockSystemLocal)); EXPECT_EQ(5u, *server.SystemCount()); EXPECT_EQ(1u, mockSystemLocal->configureCallCount); @@ -805,7 +805,7 @@ TEST_P(ServerFixture, AddSystemAfterLoad) auto result = server.AddSystem(mockSystemPlugin.value(), 100); EXPECT_FALSE(result.has_value()); - result = server.AddSystem(mockSystemLocal.get(), 100); + result = server.AddSystem(mockSystemLocal, 100); EXPECT_FALSE(result.has_value()); } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index eb377d6f0d..e8a06f57f0 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -416,6 +416,48 @@ void SimulationRunner::PublishStats() this->rootClockPub.Publish(clockMsg); } +////////////////////////////////////////////////// +void SimulationRunner::AddSystem(const SystemPluginPtr &_system, + std::optional _entity, + std::optional> _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SimulationRunner::AddSystem( + const std::shared_ptr &_system, + std::optional _entity, + std::optional> _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SimulationRunner::AddSystemImpl( + SystemInternal _system, + std::optional _entity, + std::optional> _sdf) +{ + // Call configure + if (_system.configure) + { + // Default to world entity and SDF + auto entity = _entity.has_value() ? _entity.value() + : worldEntity(this->entityCompMgr); + auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); + + _system.configure->Configure( + entity, sdf, + this->entityCompMgr, + this->eventMgr); + } + + // Update callbacks will be handled later, add to queue + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.push_back(_system); +} + ///////////////////////////////////////////////// void SimulationRunner::AddSystemToRunner(SystemInternal _system) { @@ -785,49 +827,6 @@ void SimulationRunner::Step(const UpdateInfo &_info) this->entityCompMgr.SetAllComponentsUnchanged(); } -////////////////////////////////////////////////// -void SimulationRunner::AddSystem( - const SystemPluginPtr &_system, - std::optional _entity, - std::optional _sdf) -{ - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); -} - -////////////////////////////////////////////////// -void SimulationRunner::AddSystem( - System *_system, - std::optional _entity, - std::optional _sdf) -{ - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); -} - -////////////////////////////////////////////////// -void SimulationRunner::AddSystemImpl( - SystemInternal _system, - std::optional _entity, - std::optional _sdf) -{ - // Call configure - if (_system.configure) - { - // Default to world entity and SDF - auto entity = _entity.has_value() ? _entity.value() - : worldEntity(this->entityCompMgr); - auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); - - _system.configure->Configure( - entity, sdf, - this->entityCompMgr, - this->eventMgr); - } - - // Update callbacks will be handled later, add to queue - std::lock_guard lock(this->pendingSystemsMutex); - this->pendingSystems.push_back(_system); -} - ////////////////////////////////////////////////// void SimulationRunner::LoadPlugin(const Entity _entity, const std::string &_fname, diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 1293e36703..4362044566 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -107,12 +107,13 @@ namespace ignition /// \brief Constructor /// \param[in] _system Pointer to a system. - public: explicit SystemInternal(System *_system) - : system(_system), - configure(dynamic_cast(_system)), - preupdate(dynamic_cast(_system)), - update(dynamic_cast(_system)), - postupdate(dynamic_cast(_system)) + public: explicit SystemInternal(const std::shared_ptr &_system) + : systemShared(_system), + system(_system.get()), + configure(dynamic_cast(_system.get())), + preupdate(dynamic_cast(_system.get())), + update(dynamic_cast(_system.get())), + postupdate(dynamic_cast(_system.get())) { } @@ -121,6 +122,10 @@ namespace ignition /// This will be null if the system wasn't loaded from a plugin. public: SystemPluginPtr systemPlugin; + /// \brief Pointer to a system. + /// This will be null if the system wasn't loaded from a pointer. + public: std::shared_ptr systemShared{nullptr}; + /// \brief Access this system via the `System` interface public: System *system = nullptr; @@ -189,7 +194,8 @@ namespace ignition /// SDF of the entire world. public: void AddSystem(const SystemPluginPtr &_system, std::optional _entity = std::nullopt, - std::optional _sdf = std::nullopt); + std::optional> _sdf = + std::nullopt); /// \brief Add system after the simulation runner has been instantiated /// \note This actually adds system to a queue. The system is added to the @@ -200,9 +206,10 @@ namespace ignition /// doesn't connect to an entity. /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to /// world. - public: void AddSystem(System *_system, + public: void AddSystem(const std::shared_ptr &_system, std::optional _entity = std::nullopt, - std::optional _sdf = std::nullopt); + std::optional> _sdf = + std::nullopt); /// \brief Update all the systems public: void UpdateSystems(); @@ -424,7 +431,7 @@ namespace ignition /// \param[in] _sdf SDF received from AddSystem. private: void AddSystemImpl(SystemInternal _system, std::optional _entity = std::nullopt, - std::optional _sdf = std::nullopt); + std::optional> _sdf = std::nullopt); /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; diff --git a/test/helpers/Relay.hh b/test/helpers/Relay.hh index 58a1260c6f..7ff35e2cd8 100644 --- a/test/helpers/Relay.hh +++ b/test/helpers/Relay.hh @@ -53,17 +53,15 @@ class Relay /// \brief Constructor public: Relay() { - this->mockSystem = std::make_unique(); - EXPECT_NE(nullptr, this->mockSystem); - - this->systemPtr = this->mockSystem.get(); + this->systemPtr = std::make_shared(); + EXPECT_NE(nullptr, this->systemPtr); } /// \brief Wrapper around system's pre-update callback /// \param[in] _cb Function to be called every pre-update public: Relay &OnPreUpdate(MockSystem::CallbackType _cb) { - this->mockSystem->preUpdateCallback = std::move(_cb); + this->systemPtr->preUpdateCallback = std::move(_cb); return *this; } @@ -71,7 +69,7 @@ class Relay /// \param[in] _cb Function to be called every update public: Relay &OnUpdate(MockSystem::CallbackType _cb) { - this->mockSystem->updateCallback = std::move(_cb); + this->systemPtr->updateCallback = std::move(_cb); return *this; } @@ -79,15 +77,12 @@ class Relay /// \param[in] _cb Function to be called every post-update public: Relay &OnPostUpdate(MockSystem::CallbackTypeConst _cb) { - this->mockSystem->postUpdateCallback = std::move(_cb); + this->systemPtr->postUpdateCallback = std::move(_cb); return *this; } - /// \brief Pointer to underlying mock interface - public: std::unique_ptr mockSystem{nullptr}; - - /// \brief Underlying raw pointer, for convenience - public: MockSystem *systemPtr{nullptr}; + /// \brief Pointer to system + public: std::shared_ptr systemPtr{nullptr}; }; } } diff --git a/test/integration/level_manager.cc b/test/integration/level_manager.cc index d9e8858e26..e61da0ed5a 100644 --- a/test/integration/level_manager.cc +++ b/test/integration/level_manager.cc @@ -60,7 +60,7 @@ class ModelMover: public test::Relay public: explicit ModelMover(Entity _entity): test::Relay(), entity(_entity) { using namespace std::placeholders; - this->mockSystem->preUpdateCallback = + this->systemPtr->preUpdateCallback = std::bind(&ModelMover::MoveModel, this, _1, _2); } diff --git a/test/integration/level_manager_runtime_performers.cc b/test/integration/level_manager_runtime_performers.cc index ccb8b63ba8..b873719afc 100644 --- a/test/integration/level_manager_runtime_performers.cc +++ b/test/integration/level_manager_runtime_performers.cc @@ -57,7 +57,7 @@ class ModelMover: public test::Relay public: explicit ModelMover(Entity _entity): test::Relay(), entity(_entity) { using namespace std::placeholders; - this->mockSystem->preUpdateCallback = + this->systemPtr->preUpdateCallback = std::bind(&ModelMover::MoveModel, this, _1, _2); }