From 15ebd70dd1e4d9a98222c2f5dff28f156d66ddc7 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 20:55:45 +0200 Subject: [PATCH 01/14] Removed useless code in thrift_stream Improved byte reading for large message Removed a memory leak in tcp_interface --- include/network_bridge/thrift_stream.hpp | 25 ----------- .../network_bridge/thrift_stream_queue.hpp | 41 ++++--------------- src/network_bridge.cpp | 3 ++ 3 files changed, 12 insertions(+), 57 deletions(-) diff --git a/include/network_bridge/thrift_stream.hpp b/include/network_bridge/thrift_stream.hpp index 773d77e..f2cfe11 100644 --- a/include/network_bridge/thrift_stream.hpp +++ b/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 & bytes, size_t len) = 0; virtual size_t readSome(std::vector & bytes, size_t maxlen) = 0; @@ -27,29 +25,6 @@ class Stream return -1; // not implemented } -protected: - bool recording; - std::list 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 getRecording() const - { - return R; - } - public: bool readUint8(uint8_t & b) { diff --git a/include/network_bridge/thrift_stream_queue.hpp b/include/network_bridge/thrift_stream_queue.hpp index 85d13de..498a9af 100644 --- a/include/network_bridge/thrift_stream_queue.hpp +++ b/include/network_bridge/thrift_stream_queue.hpp @@ -79,8 +79,6 @@ class QueueStream : public Stream std::unique_lock 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 @@ -160,38 +156,19 @@ class QueueStream : public Stream } - virtual bool skipToNextMessage() - { - uint8_t c0 = 0, c1 = 0, c2 = 0; - std::unique_lock 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 & bytes, size_t len) { + std::unique_lock 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; } diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 79d63dc..8ba8119 100644 --- a/src/network_bridge.cpp +++ b/src/network_bridge.cpp @@ -55,6 +55,7 @@ void NetworkBridge::initialize() void NetworkBridge::shutdown() { + RCLCPP_INFO(this->get_logger(), "NetworkBridge: Shuting down"); if (network_interface_) { network_interface_->close(); } @@ -204,6 +205,7 @@ void NetworkBridge::load_network_interface() void NetworkBridge::receive_data(std::span data) { +#if 1 auto now = std::chrono::system_clock::now(); // Decompress data @@ -270,6 +272,7 @@ void NetworkBridge::receive_data(std::span data) this->get_logger(), "Receive time: %f ms", std::chrono::duration(end - now).count()); +#endif } void NetworkBridge::send_data(std::shared_ptr manager) From 0e42faf88ba7d382c36f7fc140122127608d8c69 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 22:29:28 +0200 Subject: [PATCH 02/14] First part of commit to get a specialized management of TFs --- CMakeLists.txt | 8 +- .../network_bridge/subscription_manager.hpp | 13 ++- .../subscription_manager_tf.hpp | 81 +++++++++++++++++ package.xml | 1 + src/network_bridge.cpp | 90 +++++++++++++------ src/network_interfaces/tcp_interface.cpp | 14 +-- src/subscription_manager.cpp | 27 ++++-- src/subscription_manager_tf.cpp | 74 +++++++++++++++ 8 files changed, 264 insertions(+), 44 deletions(-) create mode 100644 include/network_bridge/subscription_manager_tf.hpp create mode 100644 src/subscription_manager_tf.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0839868..ac45593 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ 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(pluginlib REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(PkgConfig REQUIRED) @@ -39,7 +40,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,6 +56,7 @@ add_library(tcp_interface SHARED target_link_libraries(network_bridge PUBLIC ${std_msgs_TARGETS} + ${tf2_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp ${ZSTD_LIBRARIES} diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index 8002823..4467fe3 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/include/network_bridge/subscription_manager.hpp @@ -57,6 +57,8 @@ 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. * @@ -66,9 +68,9 @@ class SubscriptionManager * * @return A constant reference to the vector containing the data. */ - const std::vector & get_data(); + virtual bool get_data(std::vector & data); - bool has_data() const; + virtual bool has_data() const; void check_subscription(); @@ -81,7 +83,11 @@ 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(); + + virtual void create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos); /** * @brief Callback function for handling serialized messages. @@ -145,5 +151,6 @@ class SubscriptionManager /** * @brief The data buffer for the subscription manager. */ + std::mutex mtx; std::vector data_; }; diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp new file mode 100644 index 0000000..94907a7 --- /dev/null +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -0,0 +1,81 @@ +/* +============================================================================== +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 +#include +#include +#include +#include +#include + +#include + +/** + * @class SubscriptionManager + * @brief Manages and stores data of subscriptions to a specific topic. + * + * The SubscriptionManager class is responsible for managing and storing data of subscriptions to a specific topic. + * It provides methods to retrieve the stored data and set up subscriptions for the topic. + */ +class SubscriptionManagerTF : public SubscriptionManager +{ +public: + /** + * @brief Constructs a SubscriptionManager 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). + */ + 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(); + +protected: + void create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos) override; + + void tf2_callback( + const std::shared_ptr & tfmsg); + + rclcpp::Subscription::SharedPtr tf2_subscriber_; + + rclcpp::Serialization tf2_serialization_; + std::map, size_t> tf_id_; + tf2_msgs::msg::TFMessage tfs_; + + bool static_tf_; +}; diff --git a/package.xml b/package.xml index 915ba7b..edb5365 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ libboost-system-dev libzstd-dev std_msgs + tf2_msgs pluginlib diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 8ba8119..c16c618 100644 --- a/src/network_bridge.cpp +++ b/src/network_bridge.cpp @@ -35,6 +35,7 @@ SOFTWARE. #include #include +#include "network_bridge/subscription_manager_tf.hpp" #include "network_interfaces/network_interface_base.hpp" NetworkBridge::NetworkBridge(const std::string & node_name) @@ -130,33 +131,65 @@ void NetworkBridge::load_parameters() for (const auto & topic : topics) { std::string rate_param_name = topic + ".rate"; std::string zstd_level_param_name = topic + ".zstd_level"; + std::string is_tf_param_name = topic + ".is_tf"; + bool is_tf = (topic == "/tf") || (topic == "tf") || (topic == "/static_tf") || + (topic == "static_tf"); + bool is_static_tf = is_tf && ((topic == "/static_tf") || (topic == "static_tf")); + float rate = 1; + int zstd_level = 3; - this->declare_parameter(rate_param_name, default_rate); - this->declare_parameter(zstd_level_param_name, default_zstd_level); - - float rate; - int zstd_level; + this->declare_parameter(zstd_level_param_name, default_zstd_level); + // Add this parameter to force the tf nature if needed + this->declare_parameter(is_tf_param_name, is_tf); + this->declare_parameter(rate_param_name, default_rate); + this->get_parameter(is_tf_param_name, is_tf); this->get_parameter(rate_param_name, rate); this->get_parameter(zstd_level_param_name, zstd_level); - auto manager = std::make_shared( - shared_from_this(), topic, subscribe_namespace, - zstd_level, publish_stale_data); - sub_mgrs_.push_back(manager); - - int ms = static_cast(1000.0 / rate); - auto timer = this->create_wall_timer( - std::chrono::milliseconds(ms), - [this, manager]() { - send_data(manager); - }); - - timers_.push_back(timer); + if (is_tf) { + // Add this parameter to force the static tf nature if needed + std::string is_static_tf_param_name = topic + ".is_static_tf"; + this->declare_parameter(is_tf_param_name, is_static_tf); + + this->get_parameter(is_static_tf_param_name, is_static_tf); + + std::shared_ptr manager(new SubscriptionManagerTF( + shared_from_this(), topic, subscribe_namespace, + zstd_level, publish_stale_data, is_static_tf)); + sub_mgrs_.push_back(manager); + + // TODO: specialize this + int ms = static_cast(1000.0 / rate); + auto timer = this->create_wall_timer( + std::chrono::milliseconds(ms), + [this, manager]() { + send_data(manager); + }); + + timers_.push_back(timer); + RCLCPP_INFO( + this->get_logger(), + "TF Topic: %s, Rate: %f Hz", topic.c_str(), rate); + } else { + auto manager = std::make_shared( + shared_from_this(), topic, subscribe_namespace, + zstd_level, publish_stale_data); + sub_mgrs_.push_back(manager); + + int ms = static_cast(1000.0 / rate); + auto timer = this->create_wall_timer( + std::chrono::milliseconds(ms), + [this, manager]() { + send_data(manager); + }); + + timers_.push_back(timer); + RCLCPP_INFO( + this->get_logger(), + "Topic: %s, Rate: %f Hz", topic.c_str(), rate); + } - RCLCPP_INFO( - this->get_logger(), - "Topic: %s, Rate: %f Hz", topic.c_str(), rate); } network_check_timer_ = this->create_wall_timer( @@ -205,7 +238,10 @@ void NetworkBridge::load_network_interface() void NetworkBridge::receive_data(std::span data) { -#if 1 + if (!rclcpp::ok()) { + return; + } + auto now = std::chrono::system_clock::now(); // Decompress data @@ -265,14 +301,15 @@ void NetworkBridge::receive_data(std::span data) msg.get_rcl_serialized_message().buffer); msg.get_rcl_serialized_message().buffer_length = payload.size(); - publishers_[topic]->publish(msg); + if (rclcpp::ok()) { + publishers_[topic]->publish(msg); + } auto end = std::chrono::system_clock::now(); RCLCPP_DEBUG( this->get_logger(), "Receive time: %f ms", std::chrono::duration(end - now).count()); -#endif } void NetworkBridge::send_data(std::shared_ptr manager) @@ -285,9 +322,8 @@ void NetworkBridge::send_data(std::shared_ptr manager) return; } - const std::vector & data = manager->get_data(); - - if (data.empty()) { + std::vector data; + if (!manager->get_data(data)) { RCLCPP_WARN( this->get_logger(), "SubscriptionManager %s has no data", manager->topic_.c_str()); diff --git a/src/network_interfaces/tcp_interface.cpp b/src/network_interfaces/tcp_interface.cpp index ef9d4c8..9102df1 100644 --- a/src/network_interfaces/tcp_interface.cpp +++ b/src/network_interfaces/tcp_interface.cpp @@ -256,12 +256,14 @@ void TcpInterface::error_handler( void TcpInterface::start_receive() { - socket_->async_read_some( - boost::asio::buffer(receive_buffer_), - boost::bind( - &TcpInterface::receive, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + if (socket_) { + socket_->async_read_some( + boost::asio::buffer(receive_buffer_), + boost::bind( + &TcpInterface::receive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + } } void TcpInterface::receive(const boost::system::error_code & error, size_t rlen) diff --git a/src/subscription_manager.cpp b/src/subscription_manager.cpp index bd943c2..38d1678 100644 --- a/src/subscription_manager.cpp +++ b/src/subscription_manager.cpp @@ -45,6 +45,8 @@ SubscriptionManager::SubscriptionManager( setup_subscription(); } +SubscriptionManager::~SubscriptionManager() {} + void SubscriptionManager::setup_subscription() { if (!rclcpp::ok()) {return;} // Querying graph is fragile @@ -99,8 +101,15 @@ void SubscriptionManager::setup_subscription() msg_type_ = all_topics_and_types.at(topic)[0]; + this->create_subscription(topic, msg_type_, qos); +} + +void SubscriptionManager::create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos) +{ subscriber = node_->create_generic_subscription( - topic, msg_type_, qos, + topic, msg_type, qos, [this]( const std::shared_ptr & serialized_msg) { this->callback(serialized_msg); @@ -110,6 +119,7 @@ void SubscriptionManager::setup_subscription() void SubscriptionManager::callback( const std::shared_ptr & serialized_msg) { + std::unique_lock lock(mtx); RCLCPP_DEBUG( node_->get_logger(), "Received message on topic %s", topic_.c_str()); @@ -143,26 +153,29 @@ bool SubscriptionManager::has_data() const } -const std::vector & SubscriptionManager::get_data() +bool SubscriptionManager::get_data(std::vector & data) { + std::unique_lock lock(mtx); + data.clear(); if (!subscriber) { setup_subscription(); RCLCPP_WARN(node_->get_logger(), "Send Timer: Subscriber is not set"); - return data_; + return false; } if (!received_msg_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: No message ever received"); - return data_; + return false; } if (is_stale_ && !publish_stale_data_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: Stored data is stale"); - data_.clear(); - return data_; + data.clear(); + return false; } is_stale_ = true; - return data_; + data = data_; + return true; } diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp new file mode 100644 index 0000000..132120d --- /dev/null +++ b/src/subscription_manager_tf.cpp @@ -0,0 +1,74 @@ +/* +============================================================================== +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. +============================================================================== +*/ + +#include +#include "network_bridge/subscription_manager_tf.hpp" + +SubscriptionManagerTF::SubscriptionManagerTF( + const rclcpp::Node::SharedPtr & node, const std::string & topic, + const std::string & subscribe_namespace, int zstd_compression_level, + bool publish_stale_data, bool static_tf) +: SubscriptionManager(node, topic, subscribe_namespace, zstd_compression_level, publish_stale_data), + static_tf_(static_tf) +{ +} + +SubscriptionManagerTF::~SubscriptionManagerTF() {} + + +void SubscriptionManagerTF::create_subscription( + const std::string & topic, + const std::string & /*msg_type*/, const rclcpp::QoS & qos) +{ + tf2_subscriber_ = node_->create_subscription( + topic, qos, + [this]( + const std::shared_ptr & tfmsg) { + this->tf2_callback(tfmsg); + }); +} + +void SubscriptionManagerTF::tf2_callback( + const std::shared_ptr & tfmsg) +{ + for (size_t i = 0; i < tfmsg->transforms.size(); i++) { + const geometry_msgs::msg::TransformStamped t = tfmsg->transforms[i]; + auto id = std::make_pair(t.header.frame_id, t.child_frame_id); + auto it = tf_id_.find(id); + if (it == tf_id_.end()) { + tf_id_[id] = tfs_.transforms.size(); + tfs_.transforms.push_back(t); + RCLCPP_INFO( + node_->get_logger(), "TF list contains %lu transforms", + tfs_.transforms.size()); + } else { + tfs_.transforms[it->second] = t; + } + } + std::shared_ptr serialized_msg(new rclcpp::SerializedMessage); + tf2_serialization_.serialize_message(&tfs_, serialized_msg.get()); + callback(serialized_msg); +} From d95ae213f8c6e7b44b56d8c077804a57742c112b Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 22:30:54 +0200 Subject: [PATCH 03/14] Server tested version of TF subscriber --- CMakeLists.txt | 8 +- .../network_bridge/subscription_manager.hpp | 17 ++-- include/network_bridge/thrift_stream.hpp | 25 ------ .../network_bridge/thrift_stream_queue.hpp | 41 ++------- package.xml | 1 + src/network_bridge.cpp | 83 +++++++++++++------ src/subscription_manager.cpp | 34 ++++---- 7 files changed, 107 insertions(+), 102 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0839868..ac45593 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ 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(pluginlib REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(PkgConfig REQUIRED) @@ -39,7 +40,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,6 +56,7 @@ add_library(tcp_interface SHARED target_link_libraries(network_bridge PUBLIC ${std_msgs_TARGETS} + ${tf2_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp ${ZSTD_LIBRARIES} diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index 8002823..60e6c21 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/include/network_bridge/subscription_manager.hpp @@ -57,6 +57,8 @@ 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. * @@ -66,13 +68,12 @@ class SubscriptionManager * * @return A constant reference to the vector containing the data. */ - const std::vector & get_data(); + virtual bool get_data(std::vector & data); - bool has_data() const; + virtual bool has_data() const; - void check_subscription(); + virtual void check_subscription(); -protected: /** * @brief Sets up a subscription for a given topic. * @@ -81,7 +82,12 @@ 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(); + +protected: + virtual void create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos); /** * @brief Callback function for handling serialized messages. @@ -145,5 +151,6 @@ class SubscriptionManager /** * @brief The data buffer for the subscription manager. */ + std::mutex mtx; std::vector data_; }; diff --git a/include/network_bridge/thrift_stream.hpp b/include/network_bridge/thrift_stream.hpp index 773d77e..f2cfe11 100644 --- a/include/network_bridge/thrift_stream.hpp +++ b/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 & bytes, size_t len) = 0; virtual size_t readSome(std::vector & bytes, size_t maxlen) = 0; @@ -27,29 +25,6 @@ class Stream return -1; // not implemented } -protected: - bool recording; - std::list 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 getRecording() const - { - return R; - } - public: bool readUint8(uint8_t & b) { diff --git a/include/network_bridge/thrift_stream_queue.hpp b/include/network_bridge/thrift_stream_queue.hpp index 85d13de..498a9af 100644 --- a/include/network_bridge/thrift_stream_queue.hpp +++ b/include/network_bridge/thrift_stream_queue.hpp @@ -79,8 +79,6 @@ class QueueStream : public Stream std::unique_lock 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 @@ -160,38 +156,19 @@ class QueueStream : public Stream } - virtual bool skipToNextMessage() - { - uint8_t c0 = 0, c1 = 0, c2 = 0; - std::unique_lock 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 & bytes, size_t len) { + std::unique_lock 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; } diff --git a/package.xml b/package.xml index 915ba7b..edb5365 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ libboost-system-dev libzstd-dev std_msgs + tf2_msgs pluginlib diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 79d63dc..b6efecb 100644 --- a/src/network_bridge.cpp +++ b/src/network_bridge.cpp @@ -35,6 +35,7 @@ SOFTWARE. #include #include +#include "network_bridge/subscription_manager_tf.hpp" #include "network_interfaces/network_interface_base.hpp" NetworkBridge::NetworkBridge(const std::string & node_name) @@ -55,6 +56,7 @@ void NetworkBridge::initialize() void NetworkBridge::shutdown() { + RCLCPP_INFO(this->get_logger(), "NetworkBridge: Shuting down"); if (network_interface_) { network_interface_->close(); } @@ -129,33 +131,67 @@ void NetworkBridge::load_parameters() for (const auto & topic : topics) { std::string rate_param_name = topic + ".rate"; std::string zstd_level_param_name = topic + ".zstd_level"; + std::string is_tf_param_name = topic + ".is_tf"; + bool is_tf = (topic == "/tf") || (topic == "tf") || (topic == "/tf_static") || + (topic == "tf_static"); + bool is_static_tf = is_tf && ((topic == "/tf_static") || (topic == "tf_static")); + float rate = 1; + int zstd_level = 3; - this->declare_parameter(rate_param_name, default_rate); - this->declare_parameter(zstd_level_param_name, default_zstd_level); - - float rate; - int zstd_level; + this->declare_parameter(zstd_level_param_name, default_zstd_level); + // Add this parameter to force the tf nature if needed + this->declare_parameter(is_tf_param_name, is_tf); + this->declare_parameter(rate_param_name, default_rate); + this->get_parameter(is_tf_param_name, is_tf); this->get_parameter(rate_param_name, rate); this->get_parameter(zstd_level_param_name, zstd_level); - auto manager = std::make_shared( - shared_from_this(), topic, subscribe_namespace, - zstd_level, publish_stale_data); - sub_mgrs_.push_back(manager); - - int ms = static_cast(1000.0 / rate); - auto timer = this->create_wall_timer( - std::chrono::milliseconds(ms), - [this, manager]() { - send_data(manager); - }); - - timers_.push_back(timer); + if (is_tf) { + // Add this parameter to force the static tf nature if needed + std::string is_static_tf_param_name = topic + ".is_static_tf"; + this->declare_parameter(is_static_tf_param_name, is_static_tf); + + this->get_parameter(is_static_tf_param_name, is_static_tf); + + std::shared_ptr manager(new SubscriptionManagerTF( + shared_from_this(), topic, subscribe_namespace, + zstd_level, publish_stale_data, is_static_tf)); + manager->setup_subscription(); + sub_mgrs_.push_back(manager); + + // TODO: specialize this + int ms = static_cast(1000.0 / rate); + auto timer = this->create_wall_timer( + std::chrono::milliseconds(ms), + [this, manager]() { + send_data(manager); + }); + + timers_.push_back(timer); + RCLCPP_INFO( + this->get_logger(), + "TF Topic: %s, Rate: %f Hz", topic.c_str(), rate); + } else { + auto manager = std::make_shared( + shared_from_this(), topic, subscribe_namespace, + zstd_level, publish_stale_data); + manager->setup_subscription(); + sub_mgrs_.push_back(manager); + + int ms = static_cast(1000.0 / rate); + auto timer = this->create_wall_timer( + std::chrono::milliseconds(ms), + [this, manager]() { + send_data(manager); + }); + + timers_.push_back(timer); + RCLCPP_INFO( + this->get_logger(), + "Topic: %s, Rate: %f Hz", topic.c_str(), rate); + } - RCLCPP_INFO( - this->get_logger(), - "Topic: %s, Rate: %f Hz", topic.c_str(), rate); } network_check_timer_ = this->create_wall_timer( @@ -282,9 +318,8 @@ void NetworkBridge::send_data(std::shared_ptr manager) return; } - const std::vector & data = manager->get_data(); - - if (data.empty()) { + std::vector data; + if (!manager->get_data(data)) { RCLCPP_WARN( this->get_logger(), "SubscriptionManager %s has no data", manager->topic_.c_str()); diff --git a/src/subscription_manager.cpp b/src/subscription_manager.cpp index bd943c2..651e510 100644 --- a/src/subscription_manager.cpp +++ b/src/subscription_manager.cpp @@ -42,9 +42,10 @@ SubscriptionManager::SubscriptionManager( data_() { topic_found_ = true; // optimistic - setup_subscription(); } +SubscriptionManager::~SubscriptionManager() {} + void SubscriptionManager::setup_subscription() { if (!rclcpp::ok()) {return;} // Querying graph is fragile @@ -99,8 +100,15 @@ void SubscriptionManager::setup_subscription() msg_type_ = all_topics_and_types.at(topic)[0]; + this->create_subscription(topic, msg_type_, qos); +} + +void SubscriptionManager::create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos) +{ subscriber = node_->create_generic_subscription( - topic, msg_type_, qos, + topic, msg_type, qos, [this]( const std::shared_ptr & serialized_msg) { this->callback(serialized_msg); @@ -110,6 +118,7 @@ void SubscriptionManager::setup_subscription() void SubscriptionManager::callback( const std::shared_ptr & serialized_msg) { + std::unique_lock lock(mtx); RCLCPP_DEBUG( node_->get_logger(), "Received message on topic %s", topic_.c_str()); @@ -130,9 +139,6 @@ void SubscriptionManager::check_subscription() bool SubscriptionManager::has_data() const { - if (!subscriber) { - return false; - } if (!received_msg_) { return false; } @@ -143,26 +149,24 @@ bool SubscriptionManager::has_data() const } -const std::vector & SubscriptionManager::get_data() +bool SubscriptionManager::get_data(std::vector & data) { - if (!subscriber) { - setup_subscription(); - RCLCPP_WARN(node_->get_logger(), "Send Timer: Subscriber is not set"); - return data_; - } + std::unique_lock lock(mtx); + data.clear(); if (!received_msg_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: No message ever received"); - return data_; + return false; } if (is_stale_ && !publish_stale_data_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: Stored data is stale"); - data_.clear(); - return data_; + data.clear(); + return false; } is_stale_ = true; - return data_; + data = data_; + return true; } From f0cdaea2007a8b59f3eefe53dba73afcdab6e34b Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 22:31:34 +0200 Subject: [PATCH 04/14] Server side of subscription manager --- .../subscription_manager_tf.hpp | 83 ++++++++++++++++++ src/subscription_manager_tf.cpp | 87 +++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 include/network_bridge/subscription_manager_tf.hpp create mode 100644 src/subscription_manager_tf.cpp diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp new file mode 100644 index 0000000..642bc2b --- /dev/null +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -0,0 +1,83 @@ +/* +============================================================================== +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 +#include +#include +#include +#include +#include + +#include + +/** + * @class SubscriptionManager + * @brief Manages and stores data of subscriptions to a specific topic. + * + * The SubscriptionManager class is responsible for managing and storing data of subscriptions to a specific topic. + * It provides methods to retrieve the stored data and set up subscriptions for the topic. + */ +class SubscriptionManagerTF : public SubscriptionManager +{ +public: + /** + * @brief Constructs a SubscriptionManager 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). + */ + 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(); + + void check_subscription() override; + +protected: + void create_subscription( + const std::string & topic, + const std::string & msg_type, const rclcpp::QoS & qos) override; + + void tf2_callback( + const std::shared_ptr & tfmsg); + + rclcpp::Subscription::SharedPtr tf2_subscriber_; + + rclcpp::Serialization tf2_serialization_; + std::map, size_t> tf_id_; + tf2_msgs::msg::TFMessage tfs_; + + bool static_tf_; +}; diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp new file mode 100644 index 0000000..259aed6 --- /dev/null +++ b/src/subscription_manager_tf.cpp @@ -0,0 +1,87 @@ +/* +============================================================================== +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. +============================================================================== +*/ + +#include +#include "network_bridge/subscription_manager_tf.hpp" + +SubscriptionManagerTF::SubscriptionManagerTF( + const rclcpp::Node::SharedPtr & node, const std::string & topic, + const std::string & subscribe_namespace, int zstd_compression_level, + bool publish_stale_data, bool static_tf) +: SubscriptionManager(node, topic, subscribe_namespace, zstd_compression_level, publish_stale_data), + static_tf_(static_tf) +{ +} + +SubscriptionManagerTF::~SubscriptionManagerTF() {} + + +void SubscriptionManagerTF::check_subscription() +{ + if (!tf2_subscriber_) { + setup_subscription(); + } +} + + +void SubscriptionManagerTF::create_subscription( + const std::string & topic, + const std::string & /*msg_type*/, const rclcpp::QoS & qos) +{ + RCLCPP_INFO(node_->get_logger(), "Creating TF Subscription"); + tf2_subscriber_ = node_->create_subscription( + topic, qos, + [this]( + const std::shared_ptr & tfmsg) { + this->tf2_callback(tfmsg); + }); +} + +void SubscriptionManagerTF::tf2_callback( + const std::shared_ptr & tfmsg) +{ + bool new_tf = false; + for (size_t i = 0; i < tfmsg->transforms.size(); i++) { + const geometry_msgs::msg::TransformStamped t = tfmsg->transforms[i]; + auto id = std::make_pair(t.header.frame_id, t.child_frame_id); + auto it = tf_id_.find(id); + if (it == tf_id_.end()) { + tf_id_[id] = tfs_.transforms.size(); + tfs_.transforms.push_back(t); + new_tf = true; + } else { + tfs_.transforms[it->second] = t; + } + } + if (new_tf) { + RCLCPP_INFO( + node_->get_logger(), "TF %s list contains %lu transforms", + topic_.c_str(), tfs_.transforms.size()); + } + std::shared_ptr serialized_msg(new rclcpp::SerializedMessage); + tf2_serialization_.serialize_message(&tfs_, serialized_msg.get()); + callback(serialized_msg); +} From 3808feacf65386a6f52209334f6817ce3f43eff6 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 22:56:17 +0200 Subject: [PATCH 05/14] Fixed segfault on disconnect --- include/network_interfaces/tcp_interface.hpp | 1 + src/network_interfaces/tcp_interface.cpp | 38 ++++++++++++-------- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/include/network_interfaces/tcp_interface.hpp b/include/network_interfaces/tcp_interface.hpp index 673bc73..8a08e6a 100644 --- a/include/network_interfaces/tcp_interface.hpp +++ b/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 socket_; diff --git a/src/network_interfaces/tcp_interface.cpp b/src/network_interfaces/tcp_interface.cpp index 9102df1..176d282 100644 --- a/src/network_interfaces/tcp_interface.cpp +++ b/src/network_interfaces/tcp_interface.cpp @@ -56,6 +56,7 @@ void TcpInterface::load_parameters() void TcpInterface::open() { + shutting_down_ = false; failed_ = false; ready_ = false; io_context_.restart(); @@ -90,6 +91,7 @@ bool TcpInterface::has_failed() const void TcpInterface::close() { + shutting_down_ = true; if (acceptor_) { acceptor_->close(); } @@ -173,7 +175,7 @@ void TcpInterface::receive_thread() void TcpInterface::setup_server() { boost::system::error_code ec; - bool fatal = true; + bool fatal = !shutting_down_; tcp::endpoint endpoint(tcp::v4(), port_); @@ -182,35 +184,41 @@ void TcpInterface::setup_server() acceptor_->open(endpoint.protocol(), ec); error_handler(ec, "Failed to open acceptor", fatal); + if (shutting_down_) {return;} acceptor_->set_option(tcp::acceptor::reuse_address(true), ec); error_handler(ec, "Failed to set acceptor option", fatal); + if (shutting_down_) {return;} acceptor_->bind(endpoint, ec); error_handler(ec, "Failed to bind acceptor", fatal); + if (shutting_down_) {return;} acceptor_->listen(tcp::socket::max_listen_connections, ec); error_handler(ec, "Failed to listen on acceptor", fatal); + if (shutting_down_) {return;} RCLCPP_INFO(node_->get_logger(), "Accepting connections"); acceptor_->async_accept( *socket_, [this](const boost::system::error_code & ec) { - error_handler(ec, "Failed to accept connection", true); - RCLCPP_INFO( - node_->get_logger(), - "Accepted connection from %s:%u", - socket_->remote_endpoint().address().to_string().c_str(), - socket_->remote_endpoint().port()); - ready_ = true; - start_receive(); + error_handler(ec, "Failed to accept connection", !shutting_down_); + if (!shutting_down_) { + RCLCPP_INFO( + node_->get_logger(), + "Accepted connection from %s:%u", + socket_->remote_endpoint().address().to_string().c_str(), + socket_->remote_endpoint().port()); + ready_ = true; + start_receive(); + } }); } void TcpInterface::setup_client() { boost::system::error_code ec; - bool fatal = true; + bool fatal = !shutting_down_; tcp::endpoint endpoint( address::from_string(remote_address_), port_); @@ -242,11 +250,13 @@ void TcpInterface::error_handler( bool fatal) { if (ec) { - RCLCPP_ERROR( - node_->get_logger(), "%s: %s", - error_message.c_str(), ec.message().c_str()); + if (!shutting_down_) { + RCLCPP_ERROR( + node_->get_logger(), "%s: %s", + error_message.c_str(), ec.message().c_str()); + } - if (fatal) { + if (fatal && !shutting_down_) { RCLCPP_FATAL(node_->get_logger(), "fatal error, shutting down"); rclcpp::shutdown(); exit(1); From 2623f35d4c349461c6281a5147f07d57f3110e27 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 15 Sep 2025 23:05:07 +0200 Subject: [PATCH 06/14] Added proper comments to the header file --- .../network_bridge/subscription_manager.hpp | 25 +++++++++-- .../subscription_manager_tf.hpp | 45 ++++++++++++++++--- 2 files changed, 61 insertions(+), 9 deletions(-) diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index 03401ac..99009fa 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/include/network_bridge/subscription_manager.hpp @@ -62,16 +62,26 @@ class 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. + * This method copies the data in the provided vector under the protection + * of an internal mutex. + * Return 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 boolean flag indicating if the data is valid */ virtual bool get_data(std::vector & data); + /** + * @brief Check if data is available + * + * @return a boolean flag indicating if the data is valid + */ virtual bool has_data() const; + /** + * @brief Check if the subscription has been successful, or try to set it up + * + */ virtual void check_subscription(); /** @@ -84,6 +94,13 @@ class SubscriptionManager */ 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); diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp index 642bc2b..a022a19 100644 --- a/include/network_bridge/subscription_manager_tf.hpp +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -36,17 +36,17 @@ SOFTWARE. #include /** - * @class SubscriptionManager - * @brief Manages and stores data of subscriptions to a specific topic. + * @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 topic. - * It provides methods to retrieve the stored data and set up subscriptions for the 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 SubscriptionManager object. + * @brief Constructs a SubscriptionManagerTF object. * * This constructor initializes a SubscriptionManager object with the given parameters. * @@ -55,6 +55,7 @@ class SubscriptionManagerTF : public SubscriptionManager * @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, @@ -63,21 +64,55 @@ class SubscriptionManagerTF : public SubscriptionManager virtual ~SubscriptionManagerTF(); + /** + * @brief Check if the subscription has been successful, or try to set it up + * + */ void check_subscription() override; 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 & tfmsg); + /** + * @brief The ROS2 TF2 subscriber object. + */ rclcpp::Subscription::SharedPtr tf2_subscriber_; + /** + * @brief The ROS2 TF2 serialization object. + */ rclcpp::Serialization tf2_serialization_; + /** + * @brief Map linking (frame_id,child_frame_id) to the position in tf_s + */ std::map, 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_; }; From facad9883b4ef660a3a6cc7a706e8e26a3826f7f Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 07:46:14 +0200 Subject: [PATCH 07/14] Added a detection of inconsistent TF tree. Added configuration information in README --- README.md | 20 ++++++++++++++++++++ src/subscription_manager_tf.cpp | 12 ++++++++++++ 2 files changed, 32 insertions(+) diff --git a/README.md b/README.md index 82e27f5..ae5b01b 100644 --- a/README.md +++ b/README.md @@ -62,6 +62,26 @@ 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. +``` +/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" + + /prefix/tf: + - is_tf: True + - is_static_tf: False + - rate: 10. +``` ### 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. diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index 5373c10..30467f5 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -68,6 +68,18 @@ void SubscriptionManagerTF::tf2_callback( auto id = std::make_pair(t.header.frame_id, t.child_frame_id); auto it = tf_id_.find(id); if (it == tf_id_.end()) { + auto id_rev = std::make_pair(t.child_frame_id, t.header.frame_id); + auto it_rev = tf_id_.find(id_rev); + if (it_rev != tf_id_.end()) { + // We detected TF B->A when A->B was already in the tree. + RCLCPP_INFO( + node_->get_logger(), "Detected inconsistent TF (%s->%s) on %s. Resetting buffer.", + topic_.c_str(), t.header.frame_id.c_str(), t.child_frame_id.c_str()); + tf_id_.clear(); + tfs_.transforms.clear(); + i = 0; + continue; + } tf_id_[id] = tfs_.transforms.size(); tfs_.transforms.push_back(t); new_tf = true; From 30d7a53626a4033a6a04e63e217e6b52d19bdbc7 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 07:52:14 +0200 Subject: [PATCH 08/14] Fixed special case of the invalid TF detection --- src/subscription_manager_tf.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index 30467f5..4e6ba56 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -63,11 +63,14 @@ void SubscriptionManagerTF::tf2_callback( const std::shared_ptr & tfmsg) { bool new_tf = false; - for (size_t i = 0; i < tfmsg->transforms.size(); i++) { + size_t i = 0; + // Not using a for loop to allow a clean reset. + while (i < tfmsg->transforms.size()) { const geometry_msgs::msg::TransformStamped t = tfmsg->transforms[i]; auto id = std::make_pair(t.header.frame_id, t.child_frame_id); auto it = tf_id_.find(id); if (it == tf_id_.end()) { + // Unknown TF auto id_rev = std::make_pair(t.child_frame_id, t.header.frame_id); auto it_rev = tf_id_.find(id_rev); if (it_rev != tf_id_.end()) { @@ -84,8 +87,10 @@ void SubscriptionManagerTF::tf2_callback( tfs_.transforms.push_back(t); new_tf = true; } else { + // Known TF tfs_.transforms[it->second] = t; } + i++; } if (new_tf) { RCLCPP_INFO( From 8aaeedc27c105d56839d148a947462cc0a23a1ca Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 08:08:27 +0200 Subject: [PATCH 09/14] Added specific qos for static tf, copied from TransformListener --- CMakeLists.txt | 2 ++ package.xml | 1 + src/subscription_manager_tf.cpp | 28 ++++++++++++++++++++-------- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac45593..ac0fa25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ 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) @@ -59,6 +60,7 @@ target_link_libraries(network_bridge PUBLIC ${tf2_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp + tf2_ros::tf2_ros ${ZSTD_LIBRARIES} ) diff --git a/package.xml b/package.xml index edb5365..edfd955 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ libzstd-dev std_msgs tf2_msgs + tf2_ros pluginlib diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index 4e6ba56..afed124 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -25,6 +25,7 @@ SOFTWARE. */ #include +#include #include "network_bridge/subscription_manager_tf.hpp" SubscriptionManagerTF::SubscriptionManagerTF( @@ -49,14 +50,25 @@ void SubscriptionManagerTF::check_subscription() void SubscriptionManagerTF::create_subscription( const std::string & topic, - const std::string & /*msg_type*/, const rclcpp::QoS & qos) + const std::string & /*msg_type*/, const rclcpp::QoS & /*qos*/) { - tf2_subscriber_ = node_->create_subscription( - topic, qos, - [this]( - const std::shared_ptr & tfmsg) { - this->tf2_callback(tfmsg); - }); + if (static_tf_) { + tf2_ros::StaticListenerQoS static_qos; + tf2_subscriber_ = node_->create_subscription( + topic, static_qos, + [this]( + const std::shared_ptr & tfmsg) { + this->tf2_callback(tfmsg); + }); + } else { + tf2_ros::DynamicListenerQoS dynamic_qos; + tf2_subscriber_ = node_->create_subscription( + topic, dynamic_qos, + [this]( + const std::shared_ptr & tfmsg) { + this->tf2_callback(tfmsg); + }); + } } void SubscriptionManagerTF::tf2_callback( @@ -77,7 +89,7 @@ void SubscriptionManagerTF::tf2_callback( // We detected TF B->A when A->B was already in the tree. RCLCPP_INFO( node_->get_logger(), "Detected inconsistent TF (%s->%s) on %s. Resetting buffer.", - topic_.c_str(), t.header.frame_id.c_str(), t.child_frame_id.c_str()); + t.header.frame_id.c_str(), t.child_frame_id.c_str(), topic_.c_str()); tf_id_.clear(); tfs_.transforms.clear(); i = 0; From 546e57fabfc088e07dcccf81d8ba2c1d177cac89 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 08:16:55 +0200 Subject: [PATCH 10/14] Tried deleting the TF subscriber on error. --- src/subscription_manager_tf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index afed124..35e4c44 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -92,8 +92,8 @@ void SubscriptionManagerTF::tf2_callback( t.header.frame_id.c_str(), t.child_frame_id.c_str(), topic_.c_str()); tf_id_.clear(); tfs_.transforms.clear(); - i = 0; - continue; + tf2_subscriber_.reset(); + return; } tf_id_[id] = tfs_.transforms.size(); tfs_.transforms.push_back(t); From 7b0a85e6d7410353abae7a969350d75851d6ce2c Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 10:40:07 +0200 Subject: [PATCH 11/14] Improved management of tf_static by virtualizing is_stale. static tf are never stale. --- include/network_bridge/subscription_manager.hpp | 3 +++ include/network_bridge/subscription_manager_tf.hpp | 2 ++ src/subscription_manager.cpp | 8 ++++++-- src/subscription_manager_tf.cpp | 9 +++++++++ 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index 99009fa..a790943 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/include/network_bridge/subscription_manager.hpp @@ -105,6 +105,9 @@ class SubscriptionManager 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. * diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp index a022a19..182ffb9 100644 --- a/include/network_bridge/subscription_manager_tf.hpp +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -70,6 +70,8 @@ class SubscriptionManagerTF : public SubscriptionManager */ void check_subscription() override; + bool is_stale() const override; + protected: /** * @brief Create the subscriber diff --git a/src/subscription_manager.cpp b/src/subscription_manager.cpp index 651e510..0c50618 100644 --- a/src/subscription_manager.cpp +++ b/src/subscription_manager.cpp @@ -142,12 +142,16 @@ bool SubscriptionManager::has_data() const if (!received_msg_) { return false; } - if (is_stale_ && !publish_stale_data_) { + if (this->is_stale() && !publish_stale_data_) { return false; } return true; } +bool SubscriptionManager::is_stale() const +{ + return is_stale_; +} bool SubscriptionManager::get_data(std::vector & data) { @@ -160,7 +164,7 @@ bool SubscriptionManager::get_data(std::vector & data) } - if (is_stale_ && !publish_stale_data_) { + if (this->is_stale() && !publish_stale_data_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: Stored data is stale"); data.clear(); return false; diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index 35e4c44..ec56f9c 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -71,6 +71,15 @@ void SubscriptionManagerTF::create_subscription( } } +bool SubscriptionManagerTF::is_stale() const +{ + if (static_tf_) { + return false; + } + return SubscriptionManager::is_stale(); +} + + void SubscriptionManagerTF::tf2_callback( const std::shared_ptr & tfmsg) { From ffb990373943dbdb289cf8a0b11b9fdc2a136bc8 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 14:42:01 +0200 Subject: [PATCH 12/14] Added a parameter to exclude some TF when transmitting over network bridge --- .../subscription_manager_tf.hpp | 14 +++++ src/network_bridge.cpp | 17 ++++++- src/subscription_manager_tf.cpp | 51 +++++++++++++++++++ 3 files changed, 80 insertions(+), 2 deletions(-) diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp index 182ffb9..0088ddc 100644 --- a/include/network_bridge/subscription_manager_tf.hpp +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -29,6 +29,8 @@ SOFTWARE. #include #include #include +#include + #include #include #include @@ -72,6 +74,9 @@ class SubscriptionManagerTF : public SubscriptionManager bool is_stale() const override; + void set_include_pattern(const std::vector & pattern); + void set_exclude_pattern(const std::vector & pattern); + protected: /** * @brief Create the subscriber @@ -117,4 +122,13 @@ class SubscriptionManagerTF : public SubscriptionManager * @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 include_pattern; + /** + * @brief List of excluded tf name pattern (frame_id or child), ignored if empty + */ + std::vector exclude_pattern; }; diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 86294fb..522aaa0 100644 --- a/src/network_bridge.cpp +++ b/src/network_bridge.cpp @@ -151,14 +151,27 @@ void NetworkBridge::load_parameters() // Add this parameter to force the static tf nature if needed std::string is_static_tf_param_name = topic + ".is_static_tf"; this->declare_parameter(is_static_tf_param_name, is_static_tf); + std::string tf_include_param_name = topic + ".include"; + std::string tf_exclude_param_name = topic + ".exclude"; + std::vector tf_include, tf_exclude; + this->declare_parameter(tf_include_param_name, tf_include); + this->declare_parameter(tf_exclude_param_name, tf_exclude); this->get_parameter(is_static_tf_param_name, is_static_tf); + this->get_parameter(tf_include_param_name, tf_include); + this->get_parameter(tf_exclude_param_name, tf_exclude); - std::shared_ptr manager(new SubscriptionManagerTF( + std::shared_ptr manager(new SubscriptionManagerTF( shared_from_this(), topic, subscribe_namespace, zstd_level, publish_stale_data, is_static_tf)); + if (!tf_include.empty()) { + manager->set_include_pattern(tf_include); + } + if (!tf_exclude.empty()) { + manager->set_exclude_pattern(tf_exclude); + } manager->setup_subscription(); - sub_mgrs_.push_back(manager); + sub_mgrs_.push_back(std::static_pointer_cast(manager)); // TODO: specialize this int ms = static_cast(1000.0 / rate); diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp index ec56f9c..702ff59 100644 --- a/src/subscription_manager_tf.cpp +++ b/src/subscription_manager_tf.cpp @@ -79,6 +79,19 @@ bool SubscriptionManagerTF::is_stale() const return SubscriptionManager::is_stale(); } +void SubscriptionManagerTF::set_include_pattern(const std::vector & pattern) +{ + for (auto v: pattern) { + include_pattern.push_back(std::regex(v)); + } +} + +void SubscriptionManagerTF::set_exclude_pattern(const std::vector & pattern) +{ + for (auto v: pattern) { + exclude_pattern.push_back(std::regex(v)); + } +} void SubscriptionManagerTF::tf2_callback( const std::shared_ptr & tfmsg) @@ -88,6 +101,44 @@ void SubscriptionManagerTF::tf2_callback( // Not using a for loop to allow a clean reset. while (i < tfmsg->transforms.size()) { const geometry_msgs::msg::TransformStamped t = tfmsg->transforms[i]; + if (!exclude_pattern.empty()) { + bool matched = false; + for (auto v : exclude_pattern) { + std::smatch m; + if (std::regex_match(t.header.frame_id, m, v)) { + matched = true; + break; + } + if (std::regex_match(t.child_frame_id, m, v)) { + matched = true; + break; + } + } + if (matched) { + // Ignore this transform, it's in the exclude list + i++; + continue; + } + } + if (!include_pattern.empty()) { + bool matched = false; + for (auto v : include_pattern) { + std::smatch m; + if (std::regex_match(t.header.frame_id, m, v)) { + matched = true; + break; + } + if (std::regex_match(t.child_frame_id, m, v)) { + matched = true; + break; + } + } + if (!matched) { + // Ignore this transform, it's not in the matched list + i++; + continue; + } + } auto id = std::make_pair(t.header.frame_id, t.child_frame_id); auto it = tf_id_.find(id); if (it == tf_id_.end()) { From 7165509e34b5b4c9a60d3c4856af170e2dc42e34 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 14:49:37 +0200 Subject: [PATCH 13/14] Added documentation for the include/exclude mechanism in TF. --- README.md | 8 ++++++++ .../network_bridge/subscription_manager_tf.hpp | 17 +++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/README.md b/README.md index ae5b01b..1598db8 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,8 @@ The following configuration examples demonstrate a robot sending a message on `/ #### 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: @@ -76,11 +78,17 @@ The TF topic `/tf` or `/tf_static` are handled as a special cases. The subscribe 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 diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp index 0088ddc..0496ce9 100644 --- a/include/network_bridge/subscription_manager_tf.hpp +++ b/include/network_bridge/subscription_manager_tf.hpp @@ -72,9 +72,26 @@ class SubscriptionManagerTF : public SubscriptionManager */ 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 & 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 & pattern); protected: From 7bb0e02c51ab8b77b97591ac953b15df9ca3535a Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 23:04:30 +0200 Subject: [PATCH 14/14] Removed mutex protection in get_data, but kept an explicit flag to define if the data should be considered valid. --- include/network_bridge/subscription_manager.hpp | 9 +++------ src/network_bridge.cpp | 5 +++-- src/subscription_manager.cpp | 15 ++++++--------- 3 files changed, 12 insertions(+), 17 deletions(-) diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index a790943..07be43f 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/include/network_bridge/subscription_manager.hpp @@ -62,14 +62,12 @@ class SubscriptionManager /** * @brief Retrieves the data stored in the subscription manager. * - * This method copies the data in the provided vector under the protection - * of an internal mutex. - * Return false if no data has been received or if the data is stale and + * 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 boolean flag indicating if the data is valid + * @return a const reference to the internal data buffer */ - virtual bool get_data(std::vector & data); + virtual const std::vector & get_data(bool & is_valid); /** * @brief Check if data is available @@ -170,6 +168,5 @@ class SubscriptionManager /** * @brief The data buffer for the subscription manager. */ - std::mutex mtx; std::vector data_; }; diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 522aaa0..9841016 100644 --- a/src/network_bridge.cpp +++ b/src/network_bridge.cpp @@ -337,8 +337,9 @@ void NetworkBridge::send_data(std::shared_ptr manager) return; } - std::vector data; - if (!manager->get_data(data)) { + bool is_data_valid = false; + const std::vector & data = manager->get_data(is_data_valid); + if (data.empty() || !is_data_valid) { // This should not happen given the test above RCLCPP_WARN( this->get_logger(), "SubscriptionManager %s has no data", manager->topic_.c_str()); diff --git a/src/subscription_manager.cpp b/src/subscription_manager.cpp index 0c50618..8ed45bd 100644 --- a/src/subscription_manager.cpp +++ b/src/subscription_manager.cpp @@ -118,7 +118,6 @@ void SubscriptionManager::create_subscription( void SubscriptionManager::callback( const std::shared_ptr & serialized_msg) { - std::unique_lock lock(mtx); RCLCPP_DEBUG( node_->get_logger(), "Received message on topic %s", topic_.c_str()); @@ -153,24 +152,22 @@ bool SubscriptionManager::is_stale() const return is_stale_; } -bool SubscriptionManager::get_data(std::vector & data) +const std::vector & SubscriptionManager::get_data(bool & is_valid) { - std::unique_lock lock(mtx); - data.clear(); + is_valid = false; if (!received_msg_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: No message ever received"); - return false; + return data_; } if (this->is_stale() && !publish_stale_data_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: Stored data is stale"); - data.clear(); - return false; + return data_; } is_stale_ = true; - data = data_; - return true; + is_valid = true; + return data_; }