diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 24734e18d2..e7ac8c8e0f 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -538,8 +538,7 @@ void SimulationRunner::UpdateSystems() if (this->resetInitiated) { GZ_PROFILE("Reset"); - for (auto &system : this->systemMgr->SystemsReset()) - system->Reset(this->currentInfo, this->entityCompMgr); + this->systemMgr->Reset(this->currentInfo, this->entityCompMgr); return; } diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 78df9cbedc..5aeb4b7278 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -105,6 +106,14 @@ namespace gz /// system during the `Configure` call. public: Entity parentEntity = {kNullEntity}; + /// \brief Cached filename of the plugin used when system was loaded. + /// Used for reloading a system at runtime. + public: std::string fname = ""; + + /// \brief Cached plugin name of the plugin used when system was loaded. + /// Used for reloading a system at runtime. + public: std::string name = ""; + /// \brief Cached sdf that was used to call `Configure` on the system /// Useful for if a system needs to be reconfigured at runtime public: std::shared_ptr configureSdf = nullptr; @@ -116,4 +125,3 @@ namespace gz } // namespace sim } // namespace gz #endif // GZ_SIM_SYSTEMINTERNAL_HH_ - diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index c4f82a7f5c..f7206c9983 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -136,7 +136,6 @@ class gz::sim::SystemLoaderPrivate return false; } - this->systemPluginsAdded.insert(_plugin); return true; } @@ -151,7 +150,6 @@ class gz::sim::SystemLoaderPrivate public: std::unordered_set systemPluginPaths; /// \brief System plugins that have instances loaded via the manager. - public: std::unordered_set systemPluginsAdded; }; ////////////////////////////////////////////////// diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 34a8a5da32..0d51b18304 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -47,7 +47,11 @@ void SystemManager::LoadPlugin(const Entity _entity, // System correctly loaded from library if (system) { - this->AddSystem(system.value(), _entity, _sdf); + SystemInternal ss(system.value(), _entity); + ss.fname = _fname; + ss.name = _name; + ss.configureSdf = _sdf; + this->AddSystemImpl(ss, ss.configureSdf); gzdbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } @@ -103,6 +107,82 @@ size_t SystemManager::ActivatePendingSystems() return count; } +////////////////////////////////////////////////// +/// \brief Structure to temporarily store plugin information for reset +struct PluginInfo { + /// \brief Entity plugin is attached to + Entity entity; + /// \brief Filename of the plugin library + std::string fname; + /// \brief Name of the plugin + std::string name; + /// \brief SDF element (content of the plugin tag) + sdf::ElementPtr sdf; +}; + +////////////////////////////////////////////////// +void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +{ + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.clear(); + } + + // Clear all iterable collections of systems + this->systemsConfigure.clear(); + this->systemsReset.clear(); + this->systemsPreupdate.clear(); + this->systemsUpdate.clear(); + this->systemsPostupdate.clear(); + + std::vector pluginsToBeLoaded; + + for (auto &system : this->systems) + { + if (nullptr != system.reset) + { + // If implemented, call reset and add to pending systems. + system.reset->Reset(_info, _ecm); + + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.push_back(system); + } + } + else + { + // Cannot reset systems that were created in memory rather than + // from a plugin, because there isn't access to the constructor. + if (nullptr != system.systemShared) + { + ignwarn << "In-memory without ISystemReset detected: [" + << system.name << "]\n" + << "Systems created without plugins that do not implement Reset" + << " will not be reloaded. Reset may not work correctly\n"; + continue; + } + PluginInfo info = { + system.parentEntity, system.fname, system.name, + system.configureSdf->Clone() + }; + + pluginsToBeLoaded.push_back(info); + } + } + + this->systems.clear(); + + // Load plugins which do not implement reset after clearing this->systems + // to ensure the previous instance is destroyed before the new one is created + // and configured. + for (const auto &pluginInfo : pluginsToBeLoaded) + { + this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name, + pluginInfo.sdf); + } + this->ActivatePendingSystems(); +} + ////////////////////////////////////////////////// void SystemManager::AddSystem(const SystemPluginPtr &_system, Entity _entity, diff --git a/src/SystemManager.hh b/src/SystemManager.hh index cf80d93455..e4d9ee4e0a 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -92,12 +92,26 @@ namespace gz /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); - /// \brief Get an vector of all active systems implementing "Configure" - /// \return Vector of systems's configure interfaces. + /// \brief Perform a reset on all systems + /// + /// If a system implements the ISystemReset interface, it will be called. + // + /// Otherwise, if a system does not have the ISystemReset interface + /// implemented, and was created via loading a plugin, + /// that plugin will be reloaded. + /// + /// Otherwise, if a system is created from in-memory rather than a plugin, + /// that system will remain unaffected. + /// \param[in] _info Update info corresponding to the update time + /// \param[in] _ecm Version of the ECM reset to an initial state + public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); + + /// \brief Get a vector of all systems implementing "Configure" + /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); /// \brief Get an vector of all active systems implementing "Reset" - /// \return Vector of systems's reset interfaces. + /// \return Vector of systems' reset interfaces. public: const std::vector& SystemsReset(); /// \brief Get an vector of all active systems implementing "PreUpdate" diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 318fa8223c..274fc3c001 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1661,7 +1661,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); - _ecm.Each( [&](const Entity &_entity, diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 1045df1da8..ef68d01346 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -453,6 +453,16 @@ bool LogPlaybackPrivate::ExtractStateAndResources() } } +////////////////////////////////////////////////// +void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &) +{ + // In this case, Reset is a noop + // LogPlayback already has handling for jumps in time as part of the + // Update method. + // Leaving this function implemented but empty prevents the SystemManager + // from trying to destroy and recreate the plugin. +} + ////////////////////////////////////////////////// void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -621,6 +631,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) GZ_ADD_PLUGIN(gz::sim::systems::LogPlayback, gz::sim::System, LogPlayback::ISystemConfigure, + LogPlayback::ISystemReset, LogPlayback::ISystemUpdate) GZ_ADD_PLUGIN_ALIAS(LogPlayback, diff --git a/src/systems/log/LogPlayback.hh b/src/systems/log/LogPlayback.hh index 3d49f6195c..bc8fd28110 100644 --- a/src/systems/log/LogPlayback.hh +++ b/src/systems/log/LogPlayback.hh @@ -39,6 +39,7 @@ namespace systems class LogPlayback: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate { /// \brief Constructor @@ -53,6 +54,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index a19573daf3..bddf6d854a 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -399,6 +399,15 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, } } +////////////////////////////////////////////////// +void SceneBroadcaster::Reset(const UpdateInfo &_info, + EntityComponentManager &_manager) +{ + // Run Post Update so that GUI will be refreshed if reset is called while + // simulation is paused. + this->PostUpdate(_info, _manager); +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, const EntityComponentManager &_manager) @@ -1201,7 +1210,8 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, GZ_ADD_PLUGIN(SceneBroadcaster, gz::sim::System, SceneBroadcaster::ISystemConfigure, - SceneBroadcaster::ISystemPostUpdate) + SceneBroadcaster::ISystemPostUpdate, + SceneBroadcaster::ISystemReset) // Add plugin alias so that we can refer to the plugin without the version // namespace diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index bad06adf63..5e95c87882 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -40,7 +40,8 @@ namespace systems class SceneBroadcaster final: public System, public ISystemConfigure, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemReset { /// \brief Constructor public: SceneBroadcaster(); @@ -58,6 +59,10 @@ namespace systems public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index f5ca4d27f7..b746f13402 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -303,7 +303,6 @@ void SensorsPrivate::RunOnce() this->sensorStateChanged.clear(); } - this->sensorMaskMutex.lock(); // Check the active sensors against masked sensors. // // The internal state of a rendering sensor is not updated until the @@ -313,14 +312,16 @@ void SensorsPrivate::RunOnce() // To prevent this, add sensors that are currently being rendered to // a mask. Sensors are removed from the mask when 90% of the update // delta has passed, which will allow rendering to proceed. - for (const auto & sensor : this->activeSensors) { - // 90% of update delta (1/UpdateRate()); - auto delta = std::chrono::duration_cast< std::chrono::milliseconds>( - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); - this->sensorMask[sensor->Id()] = this->updateTime + delta; + std::unique_lock lockMask(this->sensorMaskMutex); + for (const auto & sensor : this->activeSensors) + { + // 90% of update delta (1/UpdateRate()); + auto delta = std::chrono::duration_cast( + std::chrono::duration< double >(0.9 / sensor->UpdateRate())); + this->sensorMask[sensor->Id()] = this->updateTime + delta; + } } - this->sensorMaskMutex.unlock(); { GZ_PROFILE("PreRender"); @@ -556,7 +557,7 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->eventManager = &_eventMgr; - this->dataPtr->stopConn = _eventMgr.Connect( + this->dataPtr->stopConn = this->dataPtr->eventManager->Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); this->dataPtr->forceRenderConn = _eventMgr.Connect( @@ -566,6 +567,35 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->Run(); } +////////////////////////////////////////////////// +void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) +{ + GZ_PROFILE("Sensors::Reset"); + + if (this->dataPtr->running && this->dataPtr->initialized) + { + igndbg << "Resetting Sensors\n"; + + { + std::unique_lock lock(this->dataPtr->sensorMaskMutex); + this->dataPtr->sensorMask.clear(); + } + + for (auto id : this->dataPtr->sensorIds) + { + sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + + if (nullptr == s) + { + ignwarn << "Sensor removed before reset: " << id << "\n"; + continue; + } + + s->SetNextDataUpdateTime(_info.simTime); + } + } +} + ////////////////////////////////////////////////// void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) @@ -584,14 +614,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, { GZ_PROFILE("Sensors::PostUpdate"); - // \TODO(anyone) Support rewind - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - { std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && @@ -614,36 +636,45 @@ void Sensors::PostUpdate(const UpdateInfo &_info, { this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - auto time = math::durationToSecNsec(_info.simTime); - auto t = math::secNsecToDuration(time.first, time.second); - std::vector activeSensors; - this->dataPtr->sensorMaskMutex.lock(); - for (auto id : this->dataPtr->sensorIds) { - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); - auto rs = dynamic_cast(s); - - auto it = this->dataPtr->sensorMask.find(id); - if (it != this->dataPtr->sensorMask.end()) + std::unique_lock lk(this->dataPtr->sensorMaskMutex); + for (auto id : this->dataPtr->sensorIds) { - if (it->second <= t) + sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + + if (nullptr == s) { - this->dataPtr->sensorMask.erase(it); + continue; } - else + + auto rs = dynamic_cast(s); + + if (nullptr == rs) { continue; } - } - if (rs && rs->NextDataUpdateTime() <= t) - { - activeSensors.push_back(rs); + auto it = this->dataPtr->sensorMask.find(id); + if (it != this->dataPtr->sensorMask.end()) + { + if (it->second <= _info.simTime) + { + this->dataPtr->sensorMask.erase(it); + } + else + { + continue; + } + } + + if (rs && rs->NextDataUpdateTime() <= _info.simTime) + { + activeSensors.push_back(rs); + } } } - this->dataPtr->sensorMaskMutex.unlock(); if (!activeSensors.empty() || this->dataPtr->renderUtil.PendingSensors() > 0 || @@ -662,14 +693,13 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = t; + this->dataPtr->updateTime = _info.simTime; this->dataPtr->updateAvailable = true; this->dataPtr->renderCv.notify_one(); } } } - ////////////////////////////////////////////////// void SensorsPrivate::UpdateBatteryState(const EntityComponentManager &_ecm) { @@ -910,6 +940,7 @@ bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, + Sensors::ISystemReset, Sensors::ISystemUpdate, Sensors::ISystemPostUpdate ) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index 3db5809815..caf533ce29 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -56,6 +56,7 @@ namespace systems class Sensors: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate, public ISystemPostUpdate { @@ -71,6 +72,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 78560334e7..5617904590 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -85,6 +85,7 @@ set(tests_needing_display distortion_camera.cc gpu_lidar.cc optical_tactile_plugin.cc + reset_sensors.cc rgbd_camera.cc sensors_system.cc sensors_system_battery.cc @@ -171,6 +172,12 @@ target_link_libraries(INTEGRATION_collada_world_exporter gz-common${GZ_COMMON_VER}::graphics ) +if(TARGET INTEGRATION_reset_sensors) + target_link_libraries(INTEGRATION_reset_sensors + gz-common${GZ_COMMON_VER}::graphics + ) +endif() + # The default timeout (240s) doesn't seem to be enough for this test. set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) diff --git a/test/integration/reset.cc b/test/integration/reset.cc index 528aade4f3..94f293f2cb 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -125,7 +125,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(0u, this->mockSystem->resetCallCount); @@ -142,7 +142,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_LT(poseComp->Data().Z(), 1.1f); + EXPECT_LT(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(0u, this->mockSystem->resetCallCount); @@ -180,7 +180,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(1u, this->mockSystem->resetCallCount); @@ -198,7 +198,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_LT(poseComp->Data().Z(), 1.1f); + EXPECT_LT(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(1u, this->mockSystem->resetCallCount); diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc new file mode 100644 index 0000000000..1d5e0aa24c --- /dev/null +++ b/test/integration/reset_sensors.cc @@ -0,0 +1,331 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/SystemPluginPtr.hh" + +#include +#include + +#include "plugins/MockSystem.hh" +#include "helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +constexpr double kStartingAltitude = 100.0f; +constexpr double kStartingPressure = 100129.46; + +constexpr double kEndingAltitude = 80.390198f; +constexpr double kEndingPressure = 100362.99; + +////////////////////////////////////////////////// +class ResetFixture: public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + auto plugin = sm.LoadPlugin("libMockSystem.so", + "gz::sim::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: gz::sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; + + private: sim::SystemLoader sm; +}; + +template +struct MsgReceiver +{ + std::string topic; + std::mutex msgMutex; + T lastMsg; + transport::Node node; + + std::atomic msgReceived = {false}; + + void Start(const std::string &_topic) { + this->msgReceived = false; + this->node.Subscribe(_topic, &MsgReceiver::Callback, this); + this->topic = _topic; + } + + void Stop() { + this->node.Unsubscribe(this->_topic); + } + + void Callback(const T &_msg) { + std::lock_guard lk(this->msgMutex); + this->lastMsg = _msg; + this->msgReceived = true; + } + + T Last() { + std::lock_guard lk(this->msgMutex); + return this->lastMsg; + } +}; + +///////////////////////////////////////////////// +void worldReset() +{ + gz::msgs::WorldControl req; + gz::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + transport::Node node; + + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request("/world/default/control", req, timeout, rep, result); + + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +common::Image toImage(const msgs::Image &_msg) +{ + common::Image image; + common::Image::PixelFormatType pixelFormat = + common::Image::ConvertPixelFormat( + msgs::ConvertPixelFormatType( + _msg.pixel_format_type())); + image.SetFromData( + reinterpret_cast(_msg.data().c_str()), + _msg.width(), _msg.height(), pixelFormat); + return image; +} + +///////////////////////////////////////////////// +/// This test checks that that the sensors system handles cases where entities +/// are removed and then added back +TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) +{ + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "reset_sensors.sdf"); + + serverConfig.SetSdfFile(sdfFile); + + sdf::Root root; + root.Load(sdfFile); + sim::Server server(serverConfig); + + const std::string sensorName = "air_pressure_sensor"; + auto topic = "world/default/model/box/link/link/" + "sensor/air_pressure_sensor/air_pressure"; + + auto pressureReceiver = MsgReceiver(); + auto imageReceiver = MsgReceiver(); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + + this->mockSystem->configureCallback = + [&ecm](const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + ecm = &_ecm; + }; + + // Validate update info in the reset + this->mockSystem->resetCallback = + [](const sim::UpdateInfo &_info, + sim::EntityComponentManager &) + { + EXPECT_EQ(0u, _info.iterations); + EXPECT_EQ(std::chrono::steady_clock::duration{0}, _info.simTime); + }; + + server.AddSystem(this->systemPtr); + + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + + // Verify initial conditions of the world + { + EXPECT_FLOAT_EQ(kStartingAltitude, poseComp->Data().Z()); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(0u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(0u, this->mockSystem->updateCallCount); + EXPECT_EQ(0u, this->mockSystem->postUpdateCallCount); + } + + auto current = 1u; + auto target = 2000u; + + // Run until a sensor measurement + pressureReceiver.Start(topic); + imageReceiver.Start("camera"); + while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + + EXPECT_GE(server.IterationCount().value(), current); + EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + + // Mostly green box + { + auto image = toImage(imageReceiver.Last()); + auto centerPix = image.Pixel(image.Width()/2, image.Height()/2); + EXPECT_GE(centerPix.G(), 0.3); + EXPECT_FLOAT_EQ(0.0, centerPix.R()); + EXPECT_FLOAT_EQ(0.0, centerPix.B()); + } + + // Run until 2000 steps + pressureReceiver.msgReceived = false; + imageReceiver.msgReceived = false; + server.Run(true, target - server.IterationCount().value(), false); + + // Check iterator state + EXPECT_EQ(target, server.IterationCount().value()); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(target, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(target, this->mockSystem->updateCallCount); + EXPECT_EQ(target, this->mockSystem->postUpdateCallCount); + + // Check world state + EXPECT_TRUE(pressureReceiver.msgReceived); + EXPECT_TRUE(imageReceiver.msgReceived); + EXPECT_FLOAT_EQ(kEndingAltitude, poseComp->Data().Z()); + EXPECT_FLOAT_EQ(kEndingPressure, pressureReceiver.Last().pressure()); + + { + auto image = toImage(imageReceiver.Last()); + auto centerPix = image.Pixel(image.Width()/2, image.Height()/2); + + // Gray background + EXPECT_FLOAT_EQ(centerPix.G(), centerPix.R()); + EXPECT_FLOAT_EQ(centerPix.G(), centerPix.B()); + } + + + // Send command to reset to initial state + worldReset(); + + // It takes two iterations for this to propagate, + // the first is for the message to be received and internal state setup + server.Run(true, 1, false); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(target + 1, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(target + 1, this->mockSystem->updateCallCount); + EXPECT_EQ(target + 1, this->mockSystem->postUpdateCallCount); + + imageReceiver.msgReceived = false; + pressureReceiver.msgReceived = false; + + // The second iteration is where the reset actually occurs. + server.Run(true, 1, false); + { + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + + // These should not increment, because only reset is called + EXPECT_EQ(target + 1, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(target + 1, this->mockSystem->updateCallCount); + EXPECT_EQ(target + 1, this->mockSystem->postUpdateCallCount); + + EXPECT_FLOAT_EQ(kStartingAltitude, poseComp->Data().Z()); + + // Reset does not cause messages to be sent + EXPECT_FALSE(imageReceiver.msgReceived); + EXPECT_FALSE(pressureReceiver.msgReceived); + } + + current = 2001; + target = 4001; + + while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + EXPECT_GE(server.IterationCount().value(), 1u); + EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + + // Mostly green box + { + auto image = toImage(imageReceiver.Last()); + auto centerPix = image.Pixel(image.Width()/2, image.Height()/2); + EXPECT_GE(centerPix.G(), 0.3); + EXPECT_FLOAT_EQ(0.0, centerPix.R()); + EXPECT_FLOAT_EQ(0.0, centerPix.B()); + } + + // Run until target steps + pressureReceiver.msgReceived = false; + imageReceiver.msgReceived = false; + server.Run(true, 2000 - server.IterationCount().value(), false); + + // Check iterator state + EXPECT_EQ(2000u, server.IterationCount().value()); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + EXPECT_EQ(target, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(target, this->mockSystem->updateCallCount); + EXPECT_EQ(target, this->mockSystem->postUpdateCallCount); + + // Check world state + EXPECT_TRUE(pressureReceiver.msgReceived); + EXPECT_TRUE(imageReceiver.msgReceived); + EXPECT_FLOAT_EQ(kEndingAltitude, poseComp->Data().Z()); + EXPECT_FLOAT_EQ(kEndingPressure, pressureReceiver.Last().pressure()); + + { + auto image = toImage(imageReceiver.Last()); + auto centerPix = image.Pixel(image.Width()/2, image.Height()/2); + + // Gray background + EXPECT_FLOAT_EQ(centerPix.G(), centerPix.R()); + EXPECT_FLOAT_EQ(centerPix.G(), centerPix.B()); + } + +} diff --git a/test/worlds/reset.sdf b/test/worlds/reset.sdf index ea446682bd..35095d1ed5 100644 --- a/test/worlds/reset.sdf +++ b/test/worlds/reset.sdf @@ -19,17 +19,17 @@ - 0 0 1.1 0 0 0 + 0 0 5.0 0 0 0 false - 5 5 2.5 + 1 1 1 - 5 5 2.5 + 1 1 1 0 1 0 0.8 diff --git a/test/worlds/reset_sensors.sdf b/test/worlds/reset_sensors.sdf new file mode 100644 index 0000000000..bdd32004f1 --- /dev/null +++ b/test/worlds/reset_sensors.sdf @@ -0,0 +1,137 @@ + + + + + + + + + + + ogre2 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 100.0 0 0 0 + false + + + 600 + + 100 + 0 + 0 + 100 + 0 + 100 + + + + + 1 1 1 + + + + + 1 1 1 + + + 0 1 0 1.0 + 0 1 0 1.0 + 0 1 0 1.0 + + + + 1 + 100 + true + + 0 + + + 0 + 0 + + + + + + + + + true + 10 0 100 0 0 -3.1415 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 0 0 0 0 0 0 + + 1.047 + + 200 + 200 + + + 0.1 + 100 + + + 10 + camera + + + + +