Merge pull request #513 from gazebosim/merge_13_14_20240802

Merge 13 -> main

.github/workflows/package_xml.yml

@@ -0,0 +1,11 @@
+name: Validate package.xml
+
+on:
+  pull_request:
+
+jobs:
+  package-xml:
+    runs-on: ubuntu-latest
+    name: Validate package.xml
+    steps:
+      - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy

Changelog.md

@@ -4,6 +4,25 @@
 
 ## Gazebo Transport 13.X
 
+### Gazebo Transport 13.4.0 (2024-06-18)
+
+1. Add frequency to topic CLI.
+    * [Pull request #503](https://github.com/gazebosim/gz-transport/pull/503)
+
+### Gazebo Transport 13.3.0 (2024-06-05)
+
+1. Adding option to ignore local messages
+    * [Pull request #506](https://github.com/gazebosim/gz-transport/pull/506)
+
+1. Include Python tutorial in list of tutorials
+    * [Pull request #499](https://github.com/gazebosim/gz-transport/pull/499)
+
+1. Remove python3-distutils since it's not needed on Jammy
+    * [Pull request #496](https://github.com/gazebosim/gz-transport/pull/496)
+
+1. Add package.xml
+    * [Pull request #485](https://github.com/gazebosim/gz-transport/pull/485)
+
 ### Gazebo Transport 13.2.0 (2024-04-09)
 
 1. Use relative install path for gz tool data

include/gz/transport/SubscribeOptions.hh

@@ -66,6 +66,18 @@ namespace gz
       /// \return The maximum number of messages per second.
       public: uint64_t MsgsPerSec() const;
 
+      /// \brief Set the value to ignore local messages or not.
+      /// \param[in] _ignore True when ignoring local messages
+      /// or false otherwise.
+      /// \sa IgnoreLocalMessages
+      public: void SetIgnoreLocalMessages(bool _ignore);
+
+      /// \brief Whether the local messages should be ignored.
+      /// \return true when the local messages should be ignored or
+      /// false otherwise.
+      /// \sa SetIgnoreLocalMessages
+      public: bool IgnoreLocalMessages() const;
+
 #ifdef _WIN32
 // Disable warning C4251 which is triggered by
 // std::unique_ptr

include/gz/transport/SubscriptionHandler.hh

@@ -80,6 +80,10 @@ namespace gz
       /// \return A string representation of the handler UUID.
       public: std::string HandlerUuid() const;
 
+      /// \brief Return whether local messages are ignored or not.
+      /// \return True when local messages are ignored or false otherwise.
+      public: bool IgnoreLocalMessages() const;
+
       /// \brief Check if message subscription is throttled. If so, verify
       /// whether the callback should be executed or not.
       /// \return true if the callback should be executed or false otherwise.

package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>gz-transport14</name>
+  <version>14.0.0</version>
+  <description>Gazebo Transport: Provides fast and efficient asynchronous message passing, services, and data logging.</description>
+  <maintainer email="[email protected]">Carlos Agüero</maintainer>
+  <license>Apache License 2.0</license>
+  <url type="website">https://github.com/gazebosim/gz-transport</url>
+
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <build_depend>gz-cmake4</build_depend>
+
+  <depend>gz-math8</depend>
+  <depend>gz-msgs11</depend>
+  <depend>gz-tools2</depend>
+  <depend>gz-utils3</depend>
+  <depend>libsqlite3-dev</depend>
+  <depend>libzmq3-dev</depend>
+  <depend>pkg-config</depend>
+  <depend>protobuf-dev</depend>
+  <depend>pybind11-dev</depend>
+  <depend>python3-dev</depend>
+  <depend>python3-psutil</depend>
+  <depend>python3-pytest</depend>
+  <depend>uuid</depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+</package>

src/Node.cc

@@ -338,6 +338,8 @@ bool Node::Publisher::Publish(const ProtoMsg &_msg)
     pubMsgDetails->msgCopy.reset(_msg.New());
     pubMsgDetails->msgCopy->CopyFrom(_msg);
 
+    pubMsgDetails->publisherNodeUUID = this->dataPtr->publisher.NUuid();
+
     if (subscribers.haveLocal)
     {
       for (const auto &node : subscribers.localHandlers)

src/NodeShared.cc

@@ -1850,6 +1850,14 @@ void NodeSharedPrivate::PublishThread()
     // Send the message to all the local handlers.
     for (auto &handler : msgDetails->localHandlers)
     {
+
+      // Check here if we want to ignore local publications.
+      if (handler->IgnoreLocalMessages() &&
+          msgDetails->publisherNodeUUID == handler->NodeUuid())
+      {
+        continue;
+      }
+
       try
       {
         handler->RunLocalCallback(*(msgDetails->msgCopy.get()),

src/NodeSharedPrivate.hh

@@ -167,6 +167,9 @@ namespace gz
 
                 /// \brief Information about the topic and type.
                 public: MessageInfo info;
+
+                /// \brief Publisher's node UUID.
+                public: std::string publisherNodeUUID;
               };
 
       /// \brief Publish thread used to process the pubQueue.

src/Node_TEST.cc

@@ -2020,6 +2020,41 @@ TEST(NodeTest, PubThrottled)
   reset();
 }
 
+//////////////////////////////////////////////////
+/// \brief This test creates one local publisher and subscriber and
+/// checks that no messages are received when using SetIgnoreLocalMessages
+/// is set to true.
+TEST(NodeTest, IgnoreLocalMessages)
+{
+  reset();
+
+  msgs::Int32 msg;
+  msg.set_data(data);
+
+  transport::Node node;
+
+  auto pub = node.Advertise<msgs::Int32>(g_topic);
+  EXPECT_TRUE(pub);
+
+  transport::SubscribeOptions opts;
+  EXPECT_FALSE(opts.IgnoreLocalMessages());
+  opts.SetIgnoreLocalMessages(true);
+  EXPECT_TRUE(opts.IgnoreLocalMessages());
+  EXPECT_TRUE(node.Subscribe(g_topic, cb, opts));
+
+  // Should be true the first time
+  for (auto i = 0; i < 3; ++i)
+  {
+    EXPECT_TRUE(pub.Publish(msg));
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  // No messages should be received.
+  EXPECT_EQ(0, counter);
+
+  reset();
+}
+
 //////////////////////////////////////////////////
 /// \brief This test spawns a service responser and a service requester. The
 /// requester uses a wrong type for the request argument. The test should verify

src/SubscribeOptions.cc

@@ -33,9 +33,8 @@ SubscribeOptions::SubscribeOptions()
 
 //////////////////////////////////////////////////
 SubscribeOptions::SubscribeOptions(const SubscribeOptions &_otherSubscribeOpts)
-  : dataPtr(new SubscribeOptionsPrivate())
+  : dataPtr(new SubscribeOptionsPrivate(*_otherSubscribeOpts.dataPtr))
 {
-  this->SetMsgsPerSec(_otherSubscribeOpts.MsgsPerSec());
 }
 
 //////////////////////////////////////////////////
@@ -60,3 +59,15 @@ void SubscribeOptions::SetMsgsPerSec(const uint64_t _newMsgsPerSec)
 {
   this->dataPtr->msgsPerSec = _newMsgsPerSec;
 }
+
+//////////////////////////////////////////////////
+bool SubscribeOptions::IgnoreLocalMessages() const
+{
+  return this->dataPtr->ignoreLocalMessages;
+}
+
+//////////////////////////////////////////////////
+void SubscribeOptions::SetIgnoreLocalMessages(bool _ignore)
+{
+  this->dataPtr->ignoreLocalMessages = _ignore;
+}

src/SubscribeOptionsPrivate.hh

@@ -41,6 +41,9 @@ namespace gz
 
       /// \brief Default message subscription rate.
       public: uint64_t msgsPerSec = kUnthrottled;
+
+      /// \brief Whether local messages should be ignored or not.
+      public: bool ignoreLocalMessages = false;
     };
     }
   }

src/SubscriptionHandler.cc

@@ -49,6 +49,12 @@ namespace gz
       return this->hUuid;
     }
 
+    /////////////////////////////////////////////////
+    bool SubscriptionHandlerBase::IgnoreLocalMessages() const
+    {
+      return this->opts.IgnoreLocalMessages();
+    }
+
     /////////////////////////////////////////////////
     bool SubscriptionHandlerBase::UpdateThrottling()
     {

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()
 {

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();

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.");