Add a specific management of TF topics

CMakeLists.txt

@@ -14,6 +14,8 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(PkgConfig REQUIRED)
@@ -39,7 +41,11 @@ endif()
 
 include_directories(include)
 
-add_executable(network_bridge src/network_bridge.cpp src/subscription_manager.cpp)
+add_executable(network_bridge
+    src/network_bridge.cpp
+    src/subscription_manager.cpp
+    src/subscription_manager_tf.cpp
+)
 
 add_library(udp_interface SHARED
   src/network_interfaces/udp_interface.cpp
@@ -51,8 +57,10 @@ add_library(tcp_interface SHARED
 
 target_link_libraries(network_bridge PUBLIC
   ${std_msgs_TARGETS}
+  ${tf2_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp
+  tf2_ros::tf2_ros
   ${ZSTD_LIBRARIES}
 )
 

README.md

@@ -62,6 +62,34 @@ The following configuration examples demonstrate a robot sending a message on `/
       remote_address: "192.168.1.2"
       send_port: 5001
 ```
+#### Special case: TF
+The TF topic `/tf` or `/tf_static` are handled as a special cases. The subscriber side will listen to all TF messages, accumulate them
+(similarly to a TF buffer) and send all of them at the specified rate. The behavior can be disabled or forced using the `is_tf` configuration.
+
+If some TFs need to be excluded or if the list of TFs to include is finite, one can use the include and exclude regex parameters. A transform is matched (hence excluded or included) if either the `frame_id` or `child_frame_id` are matching a pattern.
+```
+/udp_sender:
+  ros__parameters:
+    UdpInterface:
+      local_address: "192.168.1.2"
+      receive_port: 5001
+      remote_address: "192.168.1.3"
+      send_port: 5000
+
+    topics:
+      - "/prefix/tf"
+      - "/tf_static"
+
+    /prefix/tf:
+      - is_tf: True
+      - is_static_tf: False
+      - rate: 10.
+
+    /tf_static:
+      - rate: 1.0
+      - is_static_tf: True
+      - exclude: ["standoff.*", "spacer.*", ".*wheel_link", ".*cliff.*"]
+```
 
 ### Choice of protocol
 - **UDP**: Use UDP for low-latency, high-throughput communications, where occasional data loss is tolerable. Ideal for real-time telemetry data like sensor streams.

include/network_bridge/subscription_manager.hpp

@@ -57,22 +57,31 @@ class SubscriptionManager
     const std::string & subscribe_namespace, int zstd_compression_level = 3,
     bool publish_stale_data = false);
 
+  virtual ~SubscriptionManager();
+
   /**
    * @brief Retrieves the data stored in the subscription manager.
    *
-   * This method returns a constant reference to the vector containing the stored data.
-   * If no data has been received or if the data is stale and the flag publish_stale_data_ is false,
-   * an empty vector is returned.
+   * Set is_valid to false if no data has been received or if the data is stale and
+   * the flag publish_stale_data_ is false,
    *
-   * @return A constant reference to the vector containing the data.
+   * @return a const reference to the internal data buffer
    */
-  const std::vector<uint8_t> & get_data();
+  virtual const std::vector<uint8_t> & get_data(bool & is_valid);
 
-  bool has_data() const;
+  /**
+   * @brief Check if data is available
+   *
+   * @return a boolean flag indicating if the data is valid
+   */
+  virtual bool has_data() const;
 
-  void check_subscription();
+  /**
+   * @brief Check if the subscription has been successful, or try to set it up
+   *
+   */
+  virtual void check_subscription();
 
-protected:
   /**
    * @brief Sets up a subscription for a given topic.
    *
@@ -81,7 +90,21 @@ class SubscriptionManager
    * This function is called automatically in the constructor and get_data() method.
    * It fails if the topic does not exist or if there are no publishers on this topic.
    */
-  void setup_subscription();
+  virtual void setup_subscription();
+
+  /**
+   * @brief Create the subscriber
+   *
+   * This function creates the actual subscriber after setup-subscription has
+   * handled the qos and other params. Can be overloaded by specialized
+   * subscribers
+   */
+  virtual void create_subscription(
+    const std::string & topic,
+    const std::string & msg_type, const rclcpp::QoS & qos);
+
+
+  virtual bool is_stale() const;
 
   /**
    * @brief Callback function for handling serialized messages.

include/network_bridge/subscription_manager_tf.hpp

@@ -0,0 +1,151 @@
+/*
+==============================================================================
+MIT License
+
+Copyright (c) 2024 Ethan M Brown
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+==============================================================================
+*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <regex>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <network_bridge/subscription_manager.hpp>
+
+/**
+ * @class SubscriptionManagerTF
+ * @brief Manages and stores data of subscriptions to a specific TF topic.
+ *
+ * The SubscriptionManager class is responsible for managing and storing data of subscriptions to a specific TF topic.
+ * It provides methods to manage the transforms, avoiding any missed transform
+ */
+class SubscriptionManagerTF : public SubscriptionManager
+{
+public:
+  /**
+   * @brief Constructs a SubscriptionManagerTF object.
+   *
+   * This constructor initializes a SubscriptionManager object with the given parameters.
+   *
+   * @param node A pointer to the rclcpp::Node object.
+   * @param topic The topic to subscribe to.
+   * @param zstd_compression_level The compression level for Zstandard compression (default: 3).
+   * @param namespace The namespace for the subscription.
+   * @param publish_stale_data Flag indicating whether to publish stale data (default: false).
+   * @param static_tf Flag indicating whether the subscriber is a static transform .
+   */
+  SubscriptionManagerTF(
+    const rclcpp::Node::SharedPtr & node, const std::string & topic,
+    const std::string & subscribe_namespace, int zstd_compression_level = 3,
+    bool publish_stale_data = false, bool static_tf = false);
+
+  virtual ~SubscriptionManagerTF();
+
+  /**
+   * @brief Check if the subscription has been successful, or try to set it up
+   *
+   */
+  void check_subscription() override;
+
+  /**
+   * @brief Check if the subscriber data is stale, but returns always false for static_tf
+   *
+   */
+  bool is_stale() const override;
+
+  /**
+   * @brief Store the vector of tf name include pattern, and convert them to std::regex
+   *
+   * Note: a transform is matched if either the frame_id or the child_frame_id match the regex.
+   *
+   */
+  void set_include_pattern(const std::vector<std::string> & pattern);
+
+  /**
+   * @brief Store the vector of tf name exclude pattern, and convert them to std::regex
+   *
+   * Note: a transform is matched if either the frame_id or the child_frame_id match the regex.
+   *
+   */
+  void set_exclude_pattern(const std::vector<std::string> & pattern);
+
+protected:
+  /**
+   * @brief Create the subscriber
+   *
+   * This function creates the actual subscriber after setup-subscription has
+   * handled the qos and other params. Can be overloaded by specialized
+   * subscribers
+   */
+  void create_subscription(
+    const std::string & topic,
+    const std::string & msg_type, const rclcpp::QoS & qos) override;
+
+  /**
+   * @brief Callback function for handling tf2 messages.
+   *
+   * This function is called when a tf2 message is received by the subscription manager.
+   * It stores the recovered transforms in the tfs_ messages
+   *
+   * @param tfmsg A shared pointer to the tf2 message.
+   */
+  void tf2_callback(
+    const std::shared_ptr<const tf2_msgs::msg::TFMessage> & tfmsg);
+
+  /**
+   * @brief The ROS2 TF2 subscriber object.
+   */
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf2_subscriber_;
+
+  /**
+   * @brief The ROS2 TF2 serialization object.
+   */
+  rclcpp::Serialization<tf2_msgs::msg::TFMessage> tf2_serialization_;
+  /**
+   * @brief Map linking (frame_id,child_frame_id) to the position in tf_s
+   */
+  std::map<std::pair<std::string, std::string>, size_t> tf_id_;
+  /**
+   * @brief TF message grouping all the transforms received so far.
+   */
+  tf2_msgs::msg::TFMessage tfs_;
+
+  /**
+   * @brief Flag indicating if this a static tf topic
+   */
+  bool static_tf_;
+
+  /**
+   * @brief List of accepted tf name pattern (frame_id or child), ignored if empty
+   */
+  std::vector<std::regex> include_pattern;
+  /**
+   * @brief List of excluded tf name pattern (frame_id or child), ignored if empty
+   */
+  std::vector<std::regex> exclude_pattern;
+};

include/network_bridge/thrift_stream.hpp

@@ -16,8 +16,6 @@ class Stream
 
   virtual void shutdown() = 0;
 
-  virtual bool skipToNextMessage() = 0;
-
   virtual bool readBytes(std::vector<uint8_t> & bytes, size_t len) = 0;
 
   virtual size_t readSome(std::vector<uint8_t> & bytes, size_t maxlen) = 0;
@@ -27,29 +25,6 @@ class Stream
     return -1;             // not implemented
   }
 
-protected:
-  bool recording;
-  std::list<uint8_t> R;           // byte recording
-
-public:
-  void startRecording()
-  {
-    R.clear();
-    recording = true;
-    // std::cout << "Start recording " << std::endl;
-  }
-
-  void stopRecording()
-  {
-    recording = false;
-    // std::cout << "Stop recording: " << R.size() << std::endl;
-  }
-
-  const std::list<uint8_t> getRecording() const
-  {
-    return R;
-  }
-
 public:
   bool readUint8(uint8_t & b)
   {

include/network_bridge/thrift_stream_queue.hpp

@@ -79,8 +79,6 @@ class QueueStream : public Stream
     std::unique_lock<std::mutex> lock(mtx);
     Q.clear();
     finish = false;
-    R.clear();
-    recording = false;
   }
 
   void shutdown()
@@ -89,8 +87,6 @@ class QueueStream : public Stream
     finish = true;
     q_condition.notify_all();
     Q.clear();
-    recording = false;
-    R.clear();
   }
 
   template<typename iterator>
@@ -160,38 +156,19 @@ class QueueStream : public Stream
   }
 
 
-  virtual bool skipToNextMessage()
-  {
-    uint8_t c0 = 0, c1 = 0, c2 = 0;
-    std::unique_lock<std::mutex> lock(mtx);
-    c0 = getOneByte(lock);
-    c1 = getOneByte(lock);
-    c2 = getOneByte(lock);
-    while ((c0 != 0x80) || (c1 != 0x01) || (c2 != 0x00)) {
-      if (finish) {break;}
-      if (recording) {
-        R.push_back(c0);
-      }
-      c0 = c1;
-      c1 = c2;
-      c2 = getOneByte(lock);
-    }
-    Q.push_front(c2);
-    Q.push_front(c1);
-    Q.push_front(c0);
-    return true;
-  }
-
-
   virtual bool readBytes(std::vector<uint8_t> & bytes, size_t len)
   {
+    std::unique_lock<std::mutex> lock(mtx);
     bytes.clear();
-    for (size_t i = 0; i < len; i++) {
-      uint8_t b = getOneByte();
-      bytes.push_back(b);
-      if (recording) {
-        R.push_back(b);
+    while (bytes.size() < len) {
+      if (Q.empty()) {
+        q_condition.wait(lock);
+        if (finish) {
+          return false;
+        }
       }
+      bytes.push_back(Q.front());
+      Q.pop_front();
     }
     return true;
   }

include/network_interfaces/tcp_interface.hpp

@@ -107,6 +107,7 @@ class TcpInterface : public NetworkInterface
   int port_;
   bool ready_;
   bool failed_;
+  bool shutting_down_;
 
   io_context io_context_;
   std::shared_ptr<tcp::socket> socket_;