Add frequency to topic CLI. (#503)

Signed-off-by: Benjamin Perseghetti <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
gazebosim · Jun 18, 2024 · bdfb152 · bdfb152
1 parent a5af525
commit bdfb152
Show file tree

Hide file tree

Showing 3 changed files with 104 additions and 4 deletions.
diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2014 Open Source Robotics Foundation
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -15,9 +17,15 @@
  *
 */
 
+#include <algorithm>
 #include <chrono>
+#include <cmath>
+#include <condition_variable>
+#include <ctime>
 #include <functional>
 #include <iostream>
+#include <mutex>
+#include <numeric>
 #include <string>
 #include <vector>
 
@@ -345,6 +353,78 @@ extern "C" void cmdTopicEcho(const char *_topic,
   }
 }
 
+//////////////////////////////////////////////////
+extern "C" void cmdTopicFrequency(const char *_topic)
+{
+  if (!_topic || std::string(_topic).empty())
+  {
+    std::cerr << "Invalid topic. Topic must not be empty.\n";
+    return;
+  }
+  using namespace std::chrono;
+  int count = 0;
+  const int samples = 11;
+  const int window = samples - 1;
+  std::vector<int64_t> timeV;
+  std::vector<float> intervalV;
+  float sum = 0.0;
+  float dev = 0.0;
+  float mean = 0.0;
+  float stdDev = 0.0;
+
+  std::function<void(const ProtoMsg&)> cb = [&](const ProtoMsg &)
+  {
+    if (count > samples || count == 0)
+    {
+      count = 0;
+      sum = 0.0;
+      dev = 0.0;
+      timeV.clear();
+      intervalV.clear();
+    }
+    if (count < samples)
+    {
+      time_point<system_clock> now = system_clock::now();
+      duration<int64_t, std::nano> duration = now.time_since_epoch();
+      timeV.push_back(duration.count());
+    }
+    else if (count == samples)
+    {
+      for (int i = 0; i < window; ++i)
+      {
+        intervalV.push_back(static_cast<float>((timeV[i+1]
+                      - timeV[i])) / 1e+9);
+      }
+      auto [min, max] = std::minmax_element(intervalV.begin(),
+                      intervalV.end());
+      mean = std::accumulate(std::begin(intervalV),
+                      std::end(intervalV), 0.0) / window;
+      for (auto interval : intervalV)
+      {
+        dev += pow(interval - mean, 2);
+      }
+      stdDev = sqrt(dev / window);
+      std::cout << "\n" << std::endl;
+      for(int i = 0; i < window; ++i)
+      {
+        std::cout << "interval [" << i << "]:    "
+          << intervalV[i] << "s" << std::endl;
+      }
+      std::cout << "average rate: " << 1.0 / mean << std::endl;
+      std::cout << "min: " << *min << "s max: " << *max
+                << "s std dev: " << stdDev << "s window: "
+                << window << std::endl;
+    }
+    ++count;
+  };
+
+  Node node;
+  if (!node.Subscribe(_topic, cb))
+    return;
+
+  waitForShutdown();
+}
+
 //////////////////////////////////////////////////
 extern "C" const char *gzVersion()
 {

diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2014 Open Source Robotics Foundation
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -92,6 +94,10 @@ extern "C" {
 extern "C" void cmdTopicEcho(const char *_topic, const double _duration,
                              int _count, MsgOutputFormat _outputFormat);
 
+/// \brief External hook to execute 'gz topic -f' from the command line.
+/// \param[in] _topic Topic name.
+extern "C" void cmdTopicFrequency(const char *_topic);
+
 /// \brief External hook to read the library version.
 /// \return C-string representing the version. Ex.: 0.1.2
 extern "C" const char *gzVersion();

diff --git a/src/cmd/topic_main.cc b/src/cmd/topic_main.cc
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -30,7 +32,8 @@ enum class TopicCommand
   kTopicList,
   kTopicInfo,
   kTopicPub,
-  kTopicEcho
+  kTopicEcho,
+  kTopicFrequency
 };
 
 //////////////////////////////////////////////////
@@ -80,6 +83,9 @@ void runTopicCommand(const TopicOptions &_opt)
       cmdTopicEcho(_opt.topic.c_str(), _opt.duration, _opt.count,
                    _opt.msgOutputFormat);
       break;
+    case TopicCommand::kTopicFrequency:
+      cmdTopicFrequency(_opt.topic.c_str());
+      break;
     case TopicCommand::kNone:
     default:
       // In the event that there is no command, display help
@@ -130,6 +136,14 @@ R"(Output data to screen. E.g.:
   gz topic -e -t /foo)")
     ->needs(topicOpt);
 
+  command->add_flag_callback("-f,--frequency",
+    [opt](){
+      opt->command = TopicCommand::kTopicFrequency;
+    },
+R"(Calculate the frequency of a topic:
+  gz topic -f -t /foo)")
+    ->needs(topicOpt);
+
   command->add_flag_callback("--json-output",
       [opt]() { opt->msgOutputFormat = MsgOutputFormat::kJSON; },
       "Output messages in JSON format.");