From 847b1aea07691bb835ba2f59127cda8bb0109f1d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 29 Jul 2021 15:44:38 -0700 Subject: [PATCH] Support adding systems that don't come from a plugin (#936) Signed-off-by: Louise Poubel --- include/ignition/gazebo/Server.hh | 11 +++ include/ignition/gazebo/Util.hh | 6 ++ src/Server.cc | 20 +++++ src/Server_TEST.cc | 43 ++++++++++- src/SimulationRunner.cc | 74 ++++++++++++------- src/SimulationRunner.hh | 67 +++++++++++++++-- src/Util.cc | 6 ++ src/Util_TEST.cc | 27 +++++-- test/helpers/Relay.hh | 28 ++----- test/integration/level_manager.cc | 2 +- .../level_manager_runtime_performers.cc | 2 +- test/plugins/MockSystem.cc | 1 + test/plugins/MockSystem.hh | 28 +++++-- 13 files changed, 245 insertions(+), 70 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 8387399ae3..fb2e7a17ef 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -218,6 +219,16 @@ namespace ignition const SystemPluginPtr &_system, const unsigned int _worldIndex = 0); + /// \brief Add a System to the server. The server must not be running when + /// 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( + const std::shared_ptr &_system, + const unsigned int _worldIndex = 0); + /// \brief Get an Entity based on a name. /// \details If multiple entities with the same name exist, the first /// entity found will be returned. diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index a76f919bde..6f9591970a 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -130,6 +130,12 @@ namespace ignition Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity, const EntityComponentManager &_ecm); + /// \brief Get the first world entity that's found. + /// \param[in] _ecm Immutable reference to ECM. + /// \return World entity ID. + Entity IGNITION_GAZEBO_VISIBLE worldEntity( + const EntityComponentManager &_ecm); + /// \brief Helper function to remove a parent scope from a given name. /// This removes the first name found before the delimiter. /// \param[in] _name Input name possibly generated by scopedName. diff --git a/src/Server.cc b/src/Server.cc index bb188ef993..7e7de3b975 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -354,6 +354,26 @@ std::optional Server::AddSystem(const SystemPluginPtr &_system, return std::nullopt; } +////////////////////////////////////////////////// +std::optional Server::AddSystem(const std::shared_ptr &_system, + const unsigned int _worldIndex) +{ + std::lock_guard lock(this->dataPtr->runMutex); + if (this->dataPtr->running) + { + ignerr << "Cannot add system while the server is runnnng.\n"; + return false; + } + + if (_worldIndex < this->dataPtr->simRunners.size()) + { + this->dataPtr->simRunners[_worldIndex]->AddSystem(_system); + return true; + } + + return std::nullopt; +} + ////////////////////////////////////////////////// bool Server::HasEntity(const std::string &_name, const unsigned int _worldIndex) const diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 2c74b4c9b2..89e8c90387 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -721,6 +721,7 @@ TEST_P(ServerFixture, AddSystemWhileRunning) EXPECT_EQ(3u, *server.SystemCount()); + // Add system from plugin gazebo::SystemLoader systemLoader; auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so", "ignition::gazebo::MockSystem", nullptr); @@ -731,6 +732,13 @@ TEST_P(ServerFixture, AddSystemWhileRunning) EXPECT_FALSE(result.value()); EXPECT_EQ(3u, *server.SystemCount()); + // Add system pointer + auto mockSystem = std::make_shared(); + result = server.AddSystem(mockSystem); + ASSERT_TRUE(result.has_value()); + EXPECT_FALSE(result.value()); + EXPECT_EQ(3u, *server.SystemCount()); + // Stop the server std::raise(SIGTERM); @@ -750,28 +758,55 @@ TEST_P(ServerFixture, AddSystemAfterLoad) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); + // Add system from plugin gazebo::SystemLoader systemLoader; auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so", "ignition::gazebo::MockSystem", nullptr); ASSERT_TRUE(mockSystemPlugin.has_value()); + auto system = mockSystemPlugin.value()->QueryInterface(); + EXPECT_NE(system, nullptr); + auto mockSystem = dynamic_cast(system); + ASSERT_NE(mockSystem, nullptr); + EXPECT_EQ(3u, *server.SystemCount()); + EXPECT_EQ(0u, mockSystem->configureCallCount); + EXPECT_TRUE(*server.AddSystem(mockSystemPlugin.value())); + EXPECT_EQ(4u, *server.SystemCount()); + EXPECT_EQ(1u, mockSystem->configureCallCount); - auto system = mockSystemPlugin.value()->QueryInterface(); - EXPECT_NE(system, nullptr); - auto mockSystem = dynamic_cast(system); - EXPECT_NE(mockSystem, nullptr); + // Add system pointer + auto mockSystemLocal = std::make_shared(); + EXPECT_EQ(0u, mockSystemLocal->configureCallCount); + + EXPECT_TRUE(server.AddSystem(mockSystemLocal)); + EXPECT_EQ(5u, *server.SystemCount()); + EXPECT_EQ(1u, mockSystemLocal->configureCallCount); + // Check that update callbacks are called server.SetUpdatePeriod(1us); EXPECT_EQ(0u, mockSystem->preUpdateCallCount); EXPECT_EQ(0u, mockSystem->updateCallCount); EXPECT_EQ(0u, mockSystem->postUpdateCallCount); + EXPECT_EQ(0u, mockSystemLocal->preUpdateCallCount); + EXPECT_EQ(0u, mockSystemLocal->updateCallCount); + EXPECT_EQ(0u, mockSystemLocal->postUpdateCallCount); server.Run(true, 1, false); EXPECT_EQ(1u, mockSystem->preUpdateCallCount); EXPECT_EQ(1u, mockSystem->updateCallCount); EXPECT_EQ(1u, mockSystem->postUpdateCallCount); + EXPECT_EQ(1u, mockSystemLocal->preUpdateCallCount); + EXPECT_EQ(1u, mockSystemLocal->updateCallCount); + EXPECT_EQ(1u, mockSystemLocal->postUpdateCallCount); + + // Add to inexistent world + auto result = server.AddSystem(mockSystemPlugin.value(), 100); + EXPECT_FALSE(result.has_value()); + + result = server.AddSystem(mockSystemLocal, 100); + EXPECT_FALSE(result.has_value()); } ///////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 6eda31fb05..e8a06f57f0 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -416,28 +416,61 @@ void SimulationRunner::PublishStats() this->rootClockPub.Publish(clockMsg); } -///////////////////////////////////////////////// -void SimulationRunner::AddSystem(const SystemPluginPtr &_system) +////////////////////////////////////////////////// +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(const SystemPluginPtr &_system) +void SimulationRunner::AddSystemToRunner(SystemInternal _system) { - this->systems.push_back(SystemInternal(_system)); - - const auto &system = this->systems.back(); + this->systems.push_back(_system); - if (system.preupdate) - this->systemsPreupdate.push_back(system.preupdate); + if (_system.preupdate) + this->systemsPreupdate.push_back(_system.preupdate); - if (system.update) - this->systemsUpdate.push_back(system.update); + if (_system.update) + this->systemsUpdate.push_back(_system.update); - if (system.postupdate) - this->systemsPostupdate.push_back(system.postupdate); + if (_system.postupdate) + this->systemsPostupdate.push_back(_system.postupdate); } ///////////////////////////////////////////////// @@ -456,7 +489,6 @@ void SimulationRunner::ProcessSystemQueue() { this->AddSystemToRunner(system); } - this->pendingSystems.clear(); // If additional systems were added, recreate the worker threads. @@ -466,9 +498,9 @@ void SimulationRunner::ProcessSystemQueue() << this->systemsPostupdate.size() + 1 << std::endl; this->postUpdateStartBarrier = - std::make_unique(this->systemsPostupdate.size() + 1); + std::make_unique(this->systemsPostupdate.size() + 1u); this->postUpdateStopBarrier = - std::make_unique(this->systemsPostupdate.size() + 1); + std::make_unique(this->systemsPostupdate.size() + 1u); this->postUpdateThreadsRunning = true; int id = 0; @@ -807,18 +839,10 @@ void SimulationRunner::LoadPlugin(const Entity _entity, system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); } - // System correctly loaded from library, try to configure + // System correctly loaded from library if (system) { - auto systemConfig = system.value()->QueryInterface(); - if (systemConfig != nullptr) - { - systemConfig->Configure(_entity, _sdf, - this->entityCompMgr, - this->eventMgr); - } - - this->AddSystem(system.value()); + this->AddSystem(system.value(), _entity, _sdf); igndbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index e9d0316f63..4362044566 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -88,26 +89,50 @@ namespace ignition std::chrono::steady_clock::duration seek{-1}; }; - /// \brief Class to hold systems internally + /// \brief Class to hold systems internally. It supports systems loaded + /// from plugins, as well as systems created at runtime. class SystemInternal { /// \brief Constructor + /// \param[in] _systemPlugin A system loaded from a plugin. public: explicit SystemInternal(SystemPluginPtr _systemPlugin) : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), + configure(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), postupdate(systemPlugin->QueryInterface()) { } + /// \brief Constructor + /// \param[in] _system Pointer to a 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())) + { + } + /// \brief Plugin object. This manages the lifecycle of the instantiated /// class as well as the shared library. + /// 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; + /// \brief Access this system via the ISystemConfigure interface + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemConfigure *configure = nullptr; + /// \brief Access this system via the ISystemPreUpdate interface /// Will be nullptr if the System doesn't implement this interface. public: ISystemPreUpdate *preupdate = nullptr; @@ -160,9 +185,31 @@ namespace ignition /// \brief Add system after the simulation runner has been instantiated /// \note This actually adds system to a queue. The system is added to the - /// runner at the begining of the a simulation cycle (call to Run) + /// runner at the begining of the a simulation cycle (call to Run). It is + /// also responsible for calling `Configure` on the system. + /// \param[in] _system SystemPluginPtr to be added + /// \param[in] _entity Entity that system is attached to. If nullopt, + /// system is attached to a world. + /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to + /// SDF of the entire world. + public: void AddSystem(const SystemPluginPtr &_system, + std::optional _entity = 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 + /// runner at the begining of the a simulation cycle (call to Run). It is + /// also responsible for calling `Configure` on the system. /// \param[in] _system System to be added - public: void AddSystem(const SystemPluginPtr &_system); + /// \param[in] _entity Entity of system to be added. Nullopt if system + /// doesn't connect to an entity. + /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to + /// world. + public: void AddSystem(const std::shared_ptr &_system, + std::optional _entity = std::nullopt, + std::optional> _sdf = + std::nullopt); /// \brief Update all the systems public: void UpdateSystems(); @@ -339,7 +386,7 @@ namespace ignition /// \brief Actually add system to the runner /// \param[in] _system System to be added - public: void AddSystemToRunner(const SystemPluginPtr &_system); + public: void AddSystemToRunner(SystemInternal _system); /// \brief Calls AddSystemToRunner to each system that is pending to be /// added. @@ -376,6 +423,16 @@ namespace ignition /// Physics component of the world, if any. public: void UpdatePhysicsParams(); + /// \brief Implementation for AddSystem functions. This only adds systems + /// to a queue, the actual addition is performed by `AddSystemToRunner` at + /// the appropriate time. + /// \param[in] _system Generic representation of a system. + /// \param[in] _entity Entity received from AddSystem. + /// \param[in] _sdf SDF received from AddSystem. + private: void AddSystemImpl(SystemInternal _system, + std::optional _entity = 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}; @@ -387,7 +444,7 @@ namespace ignition private: std::vector systems; /// \brief Pending systems to be added to systems. - private: std::vector pendingSystems; + private: std::vector pendingSystems; /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; diff --git a/src/Util.cc b/src/Util.cc index 1526f05256..9d40af5802 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -302,6 +302,12 @@ Entity worldEntity(const Entity &_entity, return entity; } +////////////////////////////////////////////////// +Entity worldEntity(const EntityComponentManager &_ecm) +{ + return _ecm.EntityByComponents(components::World()); +} + ////////////////////////////////////////////////// std::string removeParentScope(const std::string &_name, const std::string &_delim) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 8f909cb8d1..2283468c2d 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -36,8 +37,18 @@ using namespace ignition; using namespace gazebo; +/// \brief Tests for Util.hh +class UtilTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + ///////////////////////////////////////////////// -TEST(UtilTest, ScopedName) +TEST_F(UtilTest, ScopedName) { EntityComponentManager ecm; @@ -58,6 +69,7 @@ TEST(UtilTest, ScopedName) // World auto worldEntity = ecm.CreateEntity(); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(ecm)); EXPECT_EQ(kNullEntity, gazebo::worldEntity(worldEntity, ecm)); ecm.CreateComponent(worldEntity, components::World()); ecm.CreateComponent(worldEntity, components::Name("world_name")); @@ -201,6 +213,7 @@ TEST(UtilTest, ScopedName) "world_name::actorD_name"); // World entity + EXPECT_EQ(worldEntity, gazebo::worldEntity(ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(worldEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(lightAEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(modelBEntity, ecm)); @@ -219,7 +232,7 @@ TEST(UtilTest, ScopedName) } ///////////////////////////////////////////////// -TEST(UtilTest, EntitiesFromScopedName) +TEST_F(UtilTest, EntitiesFromScopedName) { EntityComponentManager ecm; @@ -301,7 +314,7 @@ TEST(UtilTest, EntitiesFromScopedName) } ///////////////////////////////////////////////// -TEST(UtilTest, EntityTypeId) +TEST_F(UtilTest, EntityTypeId) { EntityComponentManager ecm; @@ -346,7 +359,7 @@ TEST(UtilTest, EntityTypeId) } ///////////////////////////////////////////////// -TEST(UtilTest, EntityTypeStr) +TEST_F(UtilTest, EntityTypeStr) { EntityComponentManager ecm; @@ -391,7 +404,7 @@ TEST(UtilTest, EntityTypeStr) } ///////////////////////////////////////////////// -TEST(UtilTest, RemoveParentScopedName) +TEST_F(UtilTest, RemoveParentScopedName) { EXPECT_EQ(removeParentScope("world/world_name", "/"), "world_name"); EXPECT_EQ(removeParentScope("world::world_name::light::lightA_name", "::"), @@ -406,7 +419,7 @@ TEST(UtilTest, RemoveParentScopedName) } ///////////////////////////////////////////////// -TEST(UtilTest, AsFullPath) +TEST_F(UtilTest, AsFullPath) { const std::string relativeUriUnix{"meshes/collision.dae"}; const std::string relativeUriWindows{"meshes\\collision.dae"}; @@ -490,7 +503,7 @@ TEST(UtilTest, AsFullPath) } ///////////////////////////////////////////////// -TEST(UtilTest, TopLevelModel) +TEST_F(UtilTest, TopLevelModel) { EntityComponentManager ecm; diff --git a/test/helpers/Relay.hh b/test/helpers/Relay.hh index e761d6eff6..7ff35e2cd8 100644 --- a/test/helpers/Relay.hh +++ b/test/helpers/Relay.hh @@ -19,7 +19,6 @@ #include -#include #include #include "../plugins/MockSystem.hh" @@ -54,22 +53,15 @@ class Relay /// \brief Constructor public: Relay() { - auto plugin = loader.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", nullptr); - - EXPECT_TRUE(plugin.has_value()); - this->systemPtr = plugin.value(); - - this->mockSystem = static_cast( - systemPtr->QueryInterface()); - EXPECT_NE(nullptr, this->mockSystem); + 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; } @@ -77,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; } @@ -85,18 +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 syste, - public: SystemPluginPtr systemPtr; - - /// \brief Pointer to underlying mock interface - public: MockSystem *mockSystem; - - /// \brief Used to load the system. - private: SystemLoader loader; + /// \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); } diff --git a/test/plugins/MockSystem.cc b/test/plugins/MockSystem.cc index 6315624b76..d7602c5c38 100644 --- a/test/plugins/MockSystem.cc +++ b/test/plugins/MockSystem.cc @@ -3,6 +3,7 @@ #include IGNITION_ADD_PLUGIN(ignition::gazebo::MockSystem, ignition::gazebo::System, + ignition::gazebo::MockSystem::ISystemConfigure, ignition::gazebo::MockSystem::ISystemPreUpdate, ignition::gazebo::MockSystem::ISystemUpdate, ignition::gazebo::MockSystem::ISystemPostUpdate) diff --git a/test/plugins/MockSystem.hh b/test/plugins/MockSystem.hh index fee1048f3f..d6c3df31cb 100644 --- a/test/plugins/MockSystem.hh +++ b/test/plugins/MockSystem.hh @@ -25,10 +25,12 @@ namespace ignition { namespace gazebo { class IGNITION_GAZEBO_VISIBLE MockSystem : public gazebo::System, + public gazebo::ISystemConfigure, public gazebo::ISystemPreUpdate, public gazebo::ISystemUpdate, public gazebo::ISystemPostUpdate { + public: size_t configureCallCount {0}; public: size_t preUpdateCallCount {0}; public: size_t updateCallCount {0}; public: size_t postUpdateCallCount {0}; @@ -40,33 +42,47 @@ namespace ignition { std::function; + public: std::function &, + EntityComponentManager &, + EventManager &)> + configureCallback; public: CallbackType preUpdateCallback; public: CallbackType updateCallback; public: CallbackTypeConst postUpdateCallback; + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override final + { + ++this->configureCallCount; + if (this->configureCallback) + this->configureCallback(_entity, _sdf, _ecm, _eventMgr); + } public: void PreUpdate(const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &_manager) override final + gazebo::EntityComponentManager &_ecm) override final { ++this->preUpdateCallCount; if (this->preUpdateCallback) - this->preUpdateCallback(_info, _manager); + this->preUpdateCallback(_info, _ecm); } public: void Update(const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &_manager) override final + gazebo::EntityComponentManager &_ecm) override final { ++this->updateCallCount; if (this->updateCallback) - this->updateCallback(_info, _manager); + this->updateCallback(_info, _ecm); } public: void PostUpdate(const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_manager) override final + const gazebo::EntityComponentManager &_ecm) override final { ++this->postUpdateCallCount; if (this->postUpdateCallback) - this->postUpdateCallback(_info, _manager); + this->postUpdateCallback(_info, _ecm); } }; }