From 9ca7d75b1c058a920bc260775e9a4fb0b29b2c5e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 27 Jul 2023 21:54:10 +0000 Subject: [PATCH] backport sensor threading changes, #1938 and #2025 Signed-off-by: Ian Chen --- include/gz/sim/EventManager.hh | 25 ++ src/EventManager_TEST.cc | 10 + src/systems/sensors/Sensors.cc | 273 ++++++++++----- test/integration/CMakeLists.txt | 1 + .../integration/sensors_system_update_rate.cc | 310 ++++++++++++++++++ test/worlds/sensor.sdf | 9 +- 6 files changed, 540 insertions(+), 88 deletions(-) create mode 100644 test/integration/sensors_system_update_rate.cc diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index d0e83e40cb..d4a7d302cb 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -118,6 +118,31 @@ namespace ignition } } + /// \brief Get connection count for a particular event + /// Connection count for the event + public: template + unsigned int + ConnectionCount() + { + if (this->events.find(typeid(E)) == this->events.end()) + { + return 0u; + } + + E *eventPtr = dynamic_cast(this->events[typeid(E)].get()); + // All values in the map should be derived from Event, + // so this shouldn't be an issue, but it doesn't hurt to check. + if (eventPtr != nullptr) + { + return eventPtr->ConnectionCount(); + } + else + { + ignerr << "Failed to get connection count for event: " + << typeid(E).name() << std::endl; + return 0u; + } + } /// \brief Convenience type for storing typeinfo references. private: using TypeInfoRef = std::reference_wrapper; diff --git a/src/EventManager_TEST.cc b/src/EventManager_TEST.cc index 78ef5cda90..fd0b30a95d 100644 --- a/src/EventManager_TEST.cc +++ b/src/EventManager_TEST.cc @@ -29,12 +29,17 @@ TEST(EventManager, EmitConnectTest) { EventManager eventManager; + EXPECT_EQ(0u, eventManager.ConnectionCount()); + bool paused1 = false; auto connection1 = eventManager.Connect( [&paused1](bool _paused) { paused1 = _paused; }); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); + // Emitting events causes connection callbacks to be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -47,6 +52,8 @@ TEST(EventManager, EmitConnectTest) paused2 = _paused; }); + EXPECT_EQ(2u, eventManager.ConnectionCount()); + // Multiple connections should each be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -58,9 +65,12 @@ TEST(EventManager, EmitConnectTest) // Clearing the ConnectionPtr will cause it to no longer fire. connection1.reset(); + eventManager.Emit(true); EXPECT_EQ(false, paused1); EXPECT_EQ(true, paused2); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); } /// Test that we are able to connect arbitrary events and signal them. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 78a6468d7f..97038f954e 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,7 +17,6 @@ #include "Sensors.hh" -#include #include #include #include @@ -73,7 +72,7 @@ class ignition::gazebo::systems::SensorsPrivate public: sensors::Manager sensorManager; /// \brief used to store whether rendering objects have been created. - public: bool initialized = false; + public: std::atomic initialized { false }; /// \brief Main rendering interface public: RenderUtil renderUtil; @@ -96,7 +95,7 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - public: std::map cameras; + public: std::unordered_map cameras; /// \brief Maps gazebo entity to its matching sensor ID /// @@ -111,7 +110,10 @@ class ignition::gazebo::systems::SensorsPrivate public: bool doInit { false }; /// \brief Flag to signal if rendering update is needed - public: bool updateAvailable { false }; + public: std::atomic updateAvailable { false }; + + /// \brief Flag to signal if a rendering update must be done + public: std::atomic forceUpdate { false }; /// \brief Thread that rendering will occur in public: std::thread renderThread; @@ -132,11 +134,17 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Next sensors update time + public: std::chrono::steady_clock::duration nextUpdateTime; + /// \brief Sensors to include in the next rendering iteration - public: std::vector activeSensors; + public: std::set activeSensors; + + /// \brief Sensors to be updated next + public: std::set sensorsToUpdate; /// \brief Mutex to protect sensorMask - public: std::mutex sensorMaskMutex; + public: std::mutex sensorsMutex; /// \brief Mask sensor updates for sensors currently being rendered public: std::map &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime); + + /// \brief Check if any of the sensors have connections + public: bool SensorsHaveConnections(); + /// \brief Check if sensor has subscribers /// \param[in] _sensor Sensor to check /// \return True if the sensor has subscribers, false otherwise @@ -210,7 +226,7 @@ class ignition::gazebo::systems::SensorsPrivate public: std::unordered_map modelBatteryStateChanged; /// \brief A map of sensor ids to their active state - public: std::map sensorStateChanged; + public: std::unordered_map sensorStateChanged; /// \brief Disable sensors if parent model's battery is drained /// Affects sensors that are in nested models @@ -257,11 +273,16 @@ void SensorsPrivate::WaitForInit() ////////////////////////////////////////////////// void SensorsPrivate::RunOnce() { - std::unique_lock lock(this->renderMutex); - this->renderCv.wait(lock, [this]() { - return !this->running || this->updateAvailable; - }); + std::unique_lock cvLock(this->renderMutex); + this->renderCv.wait_for(cvLock, std::chrono::microseconds(1000), [this]() + { + return !this->running || this->updateAvailable; + }); + } + + if (!this->updateAvailable) + return; if (!this->running) return; @@ -269,13 +290,22 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; + std::chrono::steady_clock::duration updateTimeApplied; IGN_PROFILE("SensorsPrivate::RunOnce"); { IGN_PROFILE("Update"); + std::unique_lock timeLock(this->renderMutex); this->renderUtil.Update(); + updateTimeApplied = this->updateTime; } - if (!this->activeSensors.empty()) + bool activeSensorsEmpty = true; + { + std::unique_lock lk(this->sensorsMutex); + activeSensorsEmpty = this->activeSensors.empty(); + } + + if (!activeSensorsEmpty || this->forceUpdate) { // disable sensors that are out of battery or re-enable sensors that are // being charged @@ -294,29 +324,10 @@ 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 - // rendering operation is complete, which can leave us in a position - // where the sensor is falsely indicating that an update is needed. - // - // 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; - } - this->sensorMaskMutex.unlock(); - { IGN_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(this->updateTime); + this->scene->SetTime(updateTimeApplied); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -327,7 +338,7 @@ void SensorsPrivate::RunOnce() // disable sensors that have no subscribers to prevent doing unnecessary // work std::unordered_set tmpDisabledSensors; - this->sensorMaskMutex.lock(); + this->sensorsMutex.lock(); for (auto id : this->sensorIds) { sensors::Sensor *s = this->sensorManager.Sensor(id); @@ -338,12 +349,16 @@ void SensorsPrivate::RunOnce() tmpDisabledSensors.insert(rs); } } - this->sensorMaskMutex.unlock(); + this->sensorsMutex.unlock(); + // safety check to see if reset occurred while we're rendering + // avoid publishing outdated data if reset occurred + std::unique_lock timeLock(this->renderMutex); + if (updateTimeApplied <= this->updateTime) { // publish data IGN_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); + this->sensorManager.RunOnce(updateTimeApplied); } // re-enble sensors @@ -366,7 +381,7 @@ void SensorsPrivate::RunOnce() } this->updateAvailable = false; - lock.unlock(); + this->forceUpdate = false; this->renderCv.notify_one(); } @@ -432,15 +447,9 @@ void Sensors::RemoveSensor(const Entity &_entity) // Locking mutex to make sure the vector is not being changed while // the rendering thread is iterating over it { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(idIter->second); - auto rs = dynamic_cast(s); - auto activeSensorIt = std::find(this->dataPtr->activeSensors.begin(), - this->dataPtr->activeSensors.end(), rs); - if (activeSensorIt != this->dataPtr->activeSensors.end()) - { - this->dataPtr->activeSensors.erase(activeSensorIt); - } + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.erase(idIter->second); + this->dataPtr->sensorsToUpdate.erase(idIter->second); } // update cameras list @@ -549,7 +558,6 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::Update"); - std::unique_lock lock(this->dataPtr->renderMutex); if (this->dataPtr->running && this->dataPtr->initialized) { this->dataPtr->renderUtil.UpdateECM(_info, _ecm); @@ -561,7 +569,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::PostUpdate"); - // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -571,9 +578,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } { - std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && - (_ecm.HasComponentType(components::Camera::typeId) || + (this->dataPtr->forceUpdate || + _ecm.HasComponentType(components::Camera::typeId) || _ecm.HasComponentType(components::DepthCamera::typeId) || _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || @@ -581,6 +588,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::BoundingBoxCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); @@ -589,57 +597,68 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { - 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()) - { - if (it->second <= t) - { - this->dataPtr->sensorMask.erase(it); - } - else - { - continue; - } - } + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + this->dataPtr->updateTime = _info.simTime; + } - if (rs && rs->NextDataUpdateTime() <= t) - { - activeSensors.push_back(rs); - } + // check connections to render events + // we will need to perform render updates if there are event subscribers + // \todo(anyone) This currently forces scene tree updates at the sim update + // rate which can be too frequent and causes a performance hit. + // We should look into throttling render updates + bool hasRenderConnections = + (this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u); + + // if nextUpdateTime is max, it probably means there are previously + // no active sensors or sensors with connections. + // In this case, check if sensors have connections now. If so, we need to + // set the nextUpdateTime + if (this->dataPtr->nextUpdateTime == + std::chrono::steady_clock::duration::max() && + this->dataPtr->SensorsHaveConnections()) + { + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); } - this->dataPtr->sensorMaskMutex.unlock(); - if (!activeSensors.empty() || - this->dataPtr->renderUtil.PendingSensors() > 0) + // notify the render thread if updates are available + if (hasRenderConnections || + this->dataPtr->nextUpdateTime <= _info.simTime || + this->dataPtr->renderUtil.PendingSensors() > 0 || + this->dataPtr->forceUpdate) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); - std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->renderCv.wait(lock, [this] { - return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); - + { + std::unique_lock cvLock(this->dataPtr->renderMutex); + this->dataPtr->renderCv.wait(cvLock, [this] { + return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + } if (!this->dataPtr->running) { return; } - this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = t; - this->dataPtr->updateAvailable = true; + { + std::unique_lock lockSensors(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors = + std::move(this->dataPtr->sensorsToUpdate); + + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); + + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; + this->dataPtr->updateAvailable = true; + } this->dataPtr->renderCv.notify_one(); } } @@ -889,6 +908,86 @@ bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const return true; } +////////////////////////////////////////////////// +std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime) +{ + _sensorsToUpdate.clear(); + std::chrono::steady_clock::duration minNextUpdateTime = + std::chrono::steady_clock::duration::max(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (!this->HasConnections(rs)) + { + continue; + } + + std::chrono::steady_clock::duration time; + // if sensor's next update tims is less or equal to current sim time then + // it's in the process of being updated by the render loop + // Set their next update time to be current time + update period + if (rs->NextDataUpdateTime() <= _currentTime) + { + time = rs->NextDataUpdateTime() + + std::chrono::duration_cast( + std::chrono::duration(1.0 / rs->UpdateRate())); + } + else + { + time = rs->NextDataUpdateTime(); + } + + if (time <= minNextUpdateTime) + { + _sensorsToUpdate.clear(); + minNextUpdateTime = time; + } + _sensorsToUpdate.insert(id); + } + return minNextUpdateTime; +} + +////////////////////////////////////////////////// +bool SensorsPrivate::SensorsHaveConnections() +{ + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (this->HasConnections(rs)) + { + return true; + } + } + return false; +} + IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemUpdate, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2606428c13..af957b4769 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -99,6 +99,7 @@ set(tests_needing_display rgbd_camera.cc sensors_system.cc sensors_system_battery.cc + sensors_system_update_rate.cc shader_param_system.cc thermal_sensor_system.cc thermal_system.cc diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc new file mode 100644 index 0000000000..b5dcf0279f --- /dev/null +++ b/test/integration/sensors_system_update_rate.cc @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/Camera.hh" +#include "gz/sim/components/DepthCamera.hh" +#include "gz/sim/components/GpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Physics.hh" +#include "gz/sim/components/RgbdCamera.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/SegmentationCamera.hh" +#include "gz/sim/components/ThermalCamera.hh" +#include "gz/sim/components/World.hh" + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; +namespace components = gz::sim::components; + +////////////////////////////////////////////////// +class SensorsFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + 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; +}; + +///////////////////////////////////////////////// +TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) +{ + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/sensor.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->configureCallback = + [&](const sim::Entity &, + const std::shared_ptr &, + sim::EntityComponentManager &_ecm, + sim::EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + transport::Node node; + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + + std::vector imageTimestamps; + unsigned int imageCount = 0u; + auto cameraCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + imageTimestamps.push_back(t); + ++imageCount; + }); + node.Subscribe(prefix + "link/camera_link/sensor/camera/image", cameraCb); + + std::vector lidarTimestamps; + unsigned int lidarCount = 0u; + auto lidarCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + lidarTimestamps.push_back(t); + ++lidarCount; + }); + node.Subscribe(prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", lidarCb); + + std::vector depthTimestamps; + unsigned int depthCount = 0u; + auto depthCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + depthTimestamps.push_back(t); + ++depthCount; + }); + node.Subscribe( + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + depthCb); + + std::vector rgbdTimestamps; + unsigned int rgbdCount = 0u; + auto rgbdCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + rgbdTimestamps.push_back(t); + ++rgbdCount; + }); + node.Subscribe( + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + rgbdCb); + + std::vector thermalTimestamps; + unsigned int thermalCount = 0u; + auto thermalCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + thermalTimestamps.push_back(t); + ++thermalCount; + }); + node.Subscribe( + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + thermalCb); + + std::vector segmentationTimestamps; + unsigned int segmentationCount = 0u; + auto segmentationCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + segmentationTimestamps.push_back(t); + ++segmentationCount; + }); + node.Subscribe( + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + segmentationCb); + + unsigned int iterations = 2000u; + server.Run(true, iterations, false); + + EXPECT_NE(nullptr, ecm); + + // get the world step size + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(sim::kNullEntity, worldEntity); + auto physicsSdf = ecm->Component(worldEntity)->Data(); + double stepSize = physicsSdf.MaxStepSize(); + EXPECT_LT(0, stepSize); + + // get the sensors update rates + auto camLinkEntity = ecm->EntityByComponents(components::Name("camera_link")); + EXPECT_NE(sim::kNullEntity, camLinkEntity); + auto camEntity = ecm->EntityByComponents(components::Name("camera"), + components::ParentEntity(camLinkEntity)); + EXPECT_NE(sim::kNullEntity, camEntity); + auto sensorSdf = ecm->Component(camEntity)->Data(); + unsigned int camRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, camRate); + + auto lidarEntity = ecm->EntityByComponents(components::Name("gpu_lidar")); + EXPECT_NE(sim::kNullEntity, lidarEntity); + sensorSdf = ecm->Component(lidarEntity)->Data(); + unsigned int lidarRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, lidarRate); + + auto depthEntity = ecm->EntityByComponents(components::Name("depth_camera")); + EXPECT_NE(sim::kNullEntity, depthEntity); + sensorSdf = ecm->Component(depthEntity)->Data(); + unsigned int depthRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, depthRate); + + auto rgbdEntity = ecm->EntityByComponents(components::Name("rgbd_camera")); + EXPECT_NE(sim::kNullEntity, rgbdEntity); + sensorSdf = ecm->Component(rgbdEntity)->Data(); + unsigned int rgbdRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, rgbdRate); + + auto thermalEntity = ecm->EntityByComponents( + components::Name("thermal_camera")); + EXPECT_NE(sim::kNullEntity, thermalEntity); + sensorSdf = ecm->Component(thermalEntity)->Data(); + unsigned int thermalRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, thermalRate); + + auto segmentationEntity = ecm->EntityByComponents( + components::Name("segmentation_camera")); + EXPECT_NE(sim::kNullEntity, segmentationEntity); + sensorSdf = ecm->Component( + segmentationEntity)->Data(); + unsigned int segmentationRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, segmentationRate); + + // compute and verify expected msg count based on update rate and sim time + double timeRan = iterations * stepSize; + + unsigned int expectedCamMsgCount = timeRan / (1.0 / camRate) + 1; + unsigned int expectedDepthMsgCount = timeRan / (1.0 / depthRate) + 1; + unsigned int expectedLidarMsgCount = timeRan / (1.0 / lidarRate) + 1; + unsigned int expectedRgbdMsgCount = timeRan / (1.0 / rgbdRate) + 1; + unsigned int expectedThermalMsgCount = timeRan / (1.0 / thermalRate) + 1; + unsigned int expectedSegmentationMsgCount = + timeRan / (1.0 / segmentationRate) + 1; + + unsigned int sleep = 0; + unsigned int maxSleep = 100; + while (sleep++ < maxSleep && + (imageCount < expectedCamMsgCount || + lidarCount < expectedLidarMsgCount || + depthCount < expectedDepthMsgCount || + rgbdCount < expectedRgbdMsgCount || + thermalCount < expectedThermalMsgCount || + segmentationCount < expectedSegmentationMsgCount)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_EQ(expectedCamMsgCount, imageCount); + EXPECT_EQ(expectedDepthMsgCount, depthCount); + EXPECT_EQ(expectedLidarMsgCount, lidarCount); + EXPECT_EQ(expectedRgbdMsgCount, rgbdCount); + EXPECT_EQ(expectedThermalMsgCount, thermalCount); + EXPECT_EQ(expectedSegmentationMsgCount, segmentationCount); + + // verify timestamps + // The first timestamp may not be 0 because the rendering + // may take some time to start and it does not block the main thread + // so start with index = 1 + // \todo(anyone) Make the sensors system thread block so we always + // generate data at t = 0? + EXPECT_FALSE(imageTimestamps.empty()); + EXPECT_FALSE(lidarTimestamps.empty()); + EXPECT_FALSE(depthTimestamps.empty()); + EXPECT_FALSE(rgbdTimestamps.empty()); + EXPECT_FALSE(thermalTimestamps.empty()); + EXPECT_FALSE(segmentationTimestamps.empty()); + for (unsigned int i = 1; i < imageTimestamps.size()-1; ++i) + { + double dt = imageTimestamps[i+1] - imageTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / camRate, dt); + } + for (unsigned int i = 1; i < lidarTimestamps.size()-1; ++i) + { + double dt = lidarTimestamps[i+1] - lidarTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / lidarRate, dt); + } + for (unsigned int i = 1; i < depthTimestamps.size()-1; ++i) + { + double dt = depthTimestamps[i+1] - depthTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / depthRate, dt); + } + for (unsigned int i = 1; i < rgbdTimestamps.size()-1; ++i) + { + double dt = rgbdTimestamps[i+1] - rgbdTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / rgbdRate, dt); + } + for (unsigned int i = 1; i < thermalTimestamps.size()-1; ++i) + { + double dt = thermalTimestamps[i+1] - thermalTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / thermalRate, dt); + } + for (unsigned int i = 1; i < segmentationTimestamps.size()-1; ++i) + { + double dt = segmentationTimestamps[i+1] - segmentationTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / segmentationRate, dt); + } +} diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index 428c86d2fc..27338807f1 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -2,7 +2,8 @@ - 0 + 0.001 + 1.0 true + 50 1.047 @@ -103,6 +105,7 @@ " + 40 @@ -129,6 +132,7 @@ + 25 1.05 @@ -152,6 +156,7 @@ + 10 1.05 @@ -168,6 +173,7 @@ + 40 1.15 @@ -184,6 +190,7 @@ + 25 1.0