From e181d3e28e107ad7f2c539914469443593f9df23 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Thu, 19 Aug 2021 19:23:06 -0300 Subject: [PATCH] Publish performance sensor metrics. (#146) Signed-off-by: Franco Cipollone Co-authored-by: Louise Poubel --- include/ignition/sensors/Sensor.hh | 15 ++++ src/Sensor.cc | 107 ++++++++++++++++++++++++++++- src/Sensor_TEST.cc | 88 +++++++++++++++++++++++- 3 files changed, 206 insertions(+), 4 deletions(-) diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 3fa15c06..fa8fe4ab 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -27,6 +27,7 @@ #pragma warning(pop) #endif +#include #include #include @@ -146,6 +147,14 @@ namespace ignition /// \return True if a valid topic was set. public: bool SetTopic(const std::string &_topic); + /// \brief Get flag state for enabling performance metrics publication. + /// \return True if performance metrics are enabled, false otherwise. + public: bool EnableMetrics() const; + + /// \brief Set flag to enable publishing performance metrics + /// \param[in] _enableMetrics True to enable. + public: void SetEnableMetrics(bool _enableMetrics); + /// \brief Get parent link of the sensor. /// \return Parent link of sensor. public: std::string Parent() const; @@ -180,6 +189,12 @@ namespace ignition public: void AddSequence(ignition::msgs::Header *_msg, const std::string &_seqKey = "default"); + /// \brief Publishes information about the performance of the sensor. + /// This method is called by Update(). + /// \param[in] _now Current time. + public: void PublishMetrics( + const std::chrono::duration &_now); + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \internal /// \brief Data pointer for private data diff --git a/src/Sensor.cc b/src/Sensor.cc index c4486116..14df12fb 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -16,17 +16,18 @@ */ #include "ignition/sensors/Sensor.hh" + +#include #include #include + #include #include +#include #include -#include - using namespace ignition::sensors; - class ignition::sensors::SensorPrivate { /// \brief Populates fields from a DOM @@ -37,6 +38,10 @@ class ignition::sensors::SensorPrivate /// \return True if a valid topic was set. public: bool SetTopic(const std::string &_topic); + /// \brief Publishes information about the performance of the sensor. + /// \param[in] _now Current simulation time. + public: void PublishMetrics(const std::chrono::duration &_now); + /// \brief id given to sensor when constructed public: SensorId id; @@ -55,12 +60,27 @@ class ignition::sensors::SensorPrivate /// \brief Pose of the sensor public: ignition::math::Pose3d pose; + /// \brief Flag to enable publishing performance metrics. + public: bool enableMetrics{false}; + /// \brief How many times the sensor will generate data per second public: double updateRate = 0.0; /// \brief What sim time should this sensor update at public: ignition::common::Time nextUpdateTime; + /// \brief Last steady clock time reading from last Update call. + public: std::chrono::time_point lastRealTime; + + /// \brief Last sim time at Update call. + public: std::chrono::duration lastUpdateTime{0}; + + /// \brief Transport node. + public: ignition::transport::Node node; + + /// \brief Publishes the PerformanceSensorMetrics message. + public: ignition::transport::Node::Publisher performanceSensorMetricsPub; + /// \brief SDF element with sensor information. public: sdf::ElementPtr sdf = nullptr; @@ -110,6 +130,8 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) } this->updateRate = _sdf.UpdateRate(); + + this->enableMetrics = _sdf.EnableMetrics(); return true; } @@ -196,6 +218,79 @@ bool SensorPrivate::SetTopic(const std::string &_topic) return true; } +////////////////////////////////////////////////// +bool Sensor::EnableMetrics() const +{ + return this->dataPtr->enableMetrics; +} + + +////////////////////////////////////////////////// +void Sensor::SetEnableMetrics(bool _enableMetrics) +{ + this->dataPtr->enableMetrics = _enableMetrics; +} + +////////////////////////////////////////////////// +void Sensor::PublishMetrics(const std::chrono::duration &_now) +{ + return this->dataPtr->PublishMetrics(_now); +} + +////////////////////////////////////////////////// +void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) +{ + if(!this->performanceSensorMetricsPub) + { + const auto validTopic = transport::TopicUtils::AsValidTopic( + this->topic + "/performance_metrics"); + if (validTopic.empty()) + { + ignerr << "Failed to set metrics sensor topic [" << topic << "]" << + std::endl; + return; + } + this->performanceSensorMetricsPub = + node.Advertise(validTopic); + } + if (!performanceSensorMetricsPub || + !performanceSensorMetricsPub.HasConnections()) + { + return; + } + + // Computes simulation update rate and real update rate. + double simUpdateRate; + double realUpdateRate; + const auto clockNow = std::chrono::steady_clock::now(); + // If lastUpdateTime == 0 means it wasn't initialized yet. + if(this->lastUpdateTime.count() > 0) + { + const double diffSimUpdate = _now.count() - + this->lastUpdateTime.count(); + simUpdateRate = 1.0 / diffSimUpdate; + const double diffRealUpdate = + std::chrono::duration_cast>( + clockNow - this->lastRealTime).count(); + realUpdateRate = diffRealUpdate < std::numeric_limits::epsilon() ? + std::numeric_limits::infinity() : 1.0 / diffRealUpdate; + } + + // Update last time values. + this->lastUpdateTime = _now; + this->lastRealTime = clockNow; + + // Fill performance sensor metrics message. + msgs::PerformanceSensorMetrics performanceSensorMetricsMsg; + performanceSensorMetricsMsg.set_name(this->name); + performanceSensorMetricsMsg.set_real_update_rate(realUpdateRate); + performanceSensorMetricsMsg.set_sim_update_rate(simUpdateRate); + performanceSensorMetricsMsg.set_nominal_update_rate(this->updateRate); + + // Publish data + performanceSensorMetricsPub.Publish(performanceSensorMetricsMsg); +} + ////////////////////////////////////////////////// ignition::math::Pose3d Sensor::Pose() const { @@ -256,6 +351,12 @@ bool Sensor::Update(const ignition::common::Time &_now, // Make the update happen result = this->Update(_now); + // Publish metrics + if (this->EnableMetrics()) + { + this->PublishMetrics(std::chrono::duration(_now.Double())); + } + if (!_force && this->dataPtr->updateRate > 0.0) { // Update the time the plugin should be loaded diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 9b4e7581..d9157ec8 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -14,11 +14,27 @@ * limitations under the License. * */ +#include + +#include +#include + #include +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + #include +#include #include -#include +#include using namespace ignition; using namespace sensors; @@ -124,6 +140,76 @@ TEST(Sensor_TEST, Topic) EXPECT_FALSE(sensor.SetTopic("")); } +class SensorUpdate : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + node.Subscribe(kPerformanceMetricTopic, + &SensorUpdate::OnPerformanceMetric, this); + } + + // Callback function for the performance metric topic. + protected: void OnPerformanceMetric( + const ignition::msgs::PerformanceSensorMetrics &_msg) + { + EXPECT_EQ(kSensorName, _msg.name()); + performanceMetricsMsgsCount++; + } + + // Sensor name. + protected: const std::string kSensorName{"sensor_test"}; + // Sensor topic. + protected: const std::string kSensorTopic{"/sensor_test"}; + // Enable metrics flag. + protected: const bool kEnableMetrics{true}; + // Topic where performance metrics are published. + protected: const std::string kPerformanceMetricTopic{ + kSensorTopic + "/performance_metrics"}; + // Number of messages to be published. + protected: const unsigned int kNumberOfMessages{10}; + // Counter for performance metrics messages published. + protected: std::atomic performanceMetricsMsgsCount{0}; + // Transport node. + protected: transport::Node node; +}; + +////////////////////////////////////////////////// +TEST_F(SensorUpdate, Update) +{ + // Create sensor. + sdf::Sensor sdfSensor; + sdfSensor.SetName(kSensorName); + sdfSensor.SetTopic(kSensorTopic); + sdfSensor.SetEnableMetrics(kEnableMetrics); + std::unique_ptr sensor = std::make_unique(); + sensor->Load(sdfSensor); + ASSERT_EQ(kSensorTopic, sensor->Topic()); + ASSERT_EQ(kEnableMetrics, sensor->EnableMetrics()); + + // Call Update() method kNumberOfMessages times. + // For each call there is also a call to Sensor::PublishMetrics() that + // publishes metrics in the correspondant topic. + for (int sec = 0 ; sec < static_cast(kNumberOfMessages) ; ++sec) + { + common::Time now{sec, 0 /*nsec*/}; + sensor->Update(now, true); + } + + int sleep = 0; + const int maxSleep = 30; + while (performanceMetricsMsgsCount < kNumberOfMessages && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + ASSERT_EQ(kNumberOfMessages, performanceMetricsMsgsCount); + + auto testSensor = dynamic_cast(sensor.get()); + EXPECT_EQ(testSensor->updateCount, performanceMetricsMsgsCount); +} + ////////////////////////////////////////////////// int main(int argc, char **argv) {