diff --git a/CMakeLists.txt b/CMakeLists.txt index 0839868..ac0fa25 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/README.md b/README.md index 82e27f5..1598db8 100644 --- a/README.md +++ b/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. diff --git a/include/network_bridge/subscription_manager.hpp b/include/network_bridge/subscription_manager.hpp index 8002823..07be43f 100644 --- a/include/network_bridge/subscription_manager.hpp +++ b/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 & get_data(); + virtual const std::vector & 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. diff --git a/include/network_bridge/subscription_manager_tf.hpp b/include/network_bridge/subscription_manager_tf.hpp new file mode 100644 index 0000000..0496ce9 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include + +#include + +/** + * @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 & 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: + /** + * @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_; + + /** + * @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/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/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/package.xml b/package.xml index 915ba7b..edfd955 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,8 @@ libboost-system-dev libzstd-dev std_msgs + tf2_msgs + tf2_ros pluginlib diff --git a/src/network_bridge.cpp b/src/network_bridge.cpp index 79d63dc..9841016 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,80 @@ 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); + 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( + 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(std::static_pointer_cast(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( @@ -204,6 +253,10 @@ void NetworkBridge::load_network_interface() void NetworkBridge::receive_data(std::span data) { + if (!rclcpp::ok()) { + return; + } + auto now = std::chrono::system_clock::now(); // Decompress data @@ -263,7 +316,9 @@ 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( @@ -282,9 +337,9 @@ void NetworkBridge::send_data(std::shared_ptr manager) return; } - const std::vector & data = manager->get_data(); - - if (data.empty()) { + 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/network_interfaces/tcp_interface.cpp b/src/network_interfaces/tcp_interface.cpp index ef9d4c8..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); @@ -256,12 +266,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..8ed45bd 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); @@ -130,26 +138,23 @@ void SubscriptionManager::check_subscription() bool SubscriptionManager::has_data() const { - if (!subscriber) { - return false; - } 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_; +} -const std::vector & SubscriptionManager::get_data() +const std::vector & SubscriptionManager::get_data(bool & is_valid) { - if (!subscriber) { - setup_subscription(); - RCLCPP_WARN(node_->get_logger(), "Send Timer: Subscriber is not set"); - return data_; - } + is_valid = false; if (!received_msg_) { RCLCPP_WARN(node_->get_logger(), "Send Timer: No message ever received"); @@ -157,12 +162,12 @@ const std::vector & SubscriptionManager::get_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 data_; } is_stale_ = true; + is_valid = true; return data_; } diff --git a/src/subscription_manager_tf.cpp b/src/subscription_manager_tf.cpp new file mode 100644 index 0000000..702ff59 --- /dev/null +++ b/src/subscription_manager_tf.cpp @@ -0,0 +1,175 @@ +/* +============================================================================== +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 +#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*/) +{ + 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); + }); + } +} + +bool SubscriptionManagerTF::is_stale() const +{ + if (static_tf_) { + return false; + } + 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) +{ + bool new_tf = false; + 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]; + 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()) { + // 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()) { + // 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.", + t.header.frame_id.c_str(), t.child_frame_id.c_str(), topic_.c_str()); + tf_id_.clear(); + tfs_.transforms.clear(); + tf2_subscriber_.reset(); + return; + } + tf_id_[id] = tfs_.transforms.size(); + tfs_.transforms.push_back(t); + new_tf = true; + } else { + // Known TF + tfs_.transforms[it->second] = t; + } + i++; + } + 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); +}