diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index 68b84343..adb948c1 100644 --- 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 #include +#include +#include +#include #include #include +#include +#include #include #include @@ -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 timeV; + std::vector intervalV; + float sum = 0.0; + float dev = 0.0; + float mean = 0.0; + float stdDev = 0.0; + + std::function 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 now = system_clock::now(); + duration 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((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 index e9a827eb..3ef433ea 100644 --- 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 index 5bca79c5..e51a76c9 100644 --- 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.");