├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── bitbots_tf_buffer └── __init__.py ├── package.xml ├── setup.py └── src └── bitbots_tf_buffer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.9) 2 | project(bitbots_tf_buffer) 3 | 4 | # Add support for C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | 10 | # Include CheckIPOSupported module 11 | include(CheckIPOSupported) 12 | 13 | # Check if IPO is supported 14 | check_ipo_supported(RESULT ipo_supported) 15 | 16 | if(ipo_supported) 17 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION TRUE) 18 | # Set the number of parallel LTO jobs 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -flto=4") 20 | endif() 21 | 22 | 23 | set(PYBIND11_PYTHON_VERSION 3) 24 | set(PYBIND11_FINDPYTHON ON) 25 | find_package(ament_cmake REQUIRED) 26 | find_package(backward_ros REQUIRED) 27 | find_package(pybind11 REQUIRED) 28 | find_package(Python3 REQUIRED COMPONENTS Interpreter Development) 29 | find_package(rcl REQUIRED) 30 | find_package(rclcpp REQUIRED) 31 | find_package(ros2_python_extension REQUIRED) 32 | find_package(tf2 REQUIRED) 33 | find_package(tf2_ros REQUIRED) 34 | 35 | add_compile_options(-Wall -Wno-unused) 36 | 37 | pybind11_add_module(cpp_wrapper SHARED src/bitbots_tf_buffer.cpp) 38 | 39 | ament_target_dependencies( 40 | cpp_wrapper 41 | PUBLIC 42 | rclcpp 43 | rcl 44 | tf2 45 | tf2_ros 46 | ros2_python_extension) 47 | 48 | target_link_libraries(cpp_wrapper PRIVATE pybind11::module) 49 | 50 | ament_python_install_package(${PROJECT_NAME}) 51 | 52 | ament_get_python_install_dir(PYTHON_INSTALL_DIR) 53 | 54 | install(TARGETS cpp_wrapper DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}") 55 | 56 | ament_package() 57 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Hamburg Bit-Bots 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Bit-Bots TF Buffer 2 | 3 | This is a nearly drop-in replacement for `tf2_ros.Buffer` in Python. It wraps a C++ node that holds the tf buffer and listener. The interface should be the same as the original `tf2_ros.Buffer` and `tf2_ros.TransformListener`, except for e.g. qos settings that are not supported for now. 4 | 5 | ## Why? 6 | 7 | Sadly rclpy is very slow. 8 | This is not only a Python problem, but also a problem of the underlying rclpy implementation. 9 | 10 | TF2 is used in many nodes. 11 | The amount of callbacks that are triggered by TF2 can be very high, especially if there are multiple sources of TF data and you operate on a high frequency. A simple ROS 2 rclpy node can easily max out a CPU core just by processing TF data. This is especially unlucky, if TF is only used at a few places in the code for e.g. low frequency navigation or behavior operations. 12 | 13 | This package aims to solve this problem by moving the TF buffer and listener to a C++ node that shares the process with the rclpy node. This way, the TF callbacks are processed in C++ and the Python node only needs to query the buffer for the latest transforms using a simple and performant pybind11 interface when needed. 14 | 15 | While spinning up an additional Node is not ideal, the performance gain is significant and the overhead of the additional node is negligible compared to the performance gain. 16 | 17 | In addition to that, this solution also reduces the amount of executor deadlock scenarios and enables the usage of an single threaded executor for the rclpy node instead of the multi-threaded executor in many cases resulting in further performance gains. 18 | 19 | ## Usage 20 | 21 | Replace `from tf2_ros import Buffer, TransformListener` with `from bitbots_tf_buffer import 22 | Buffer, TransformListener`. 23 | 24 | ## Installation 25 | 26 | First we need to clone the repository as well as one of its dependencies: 27 | 28 | ```bash 29 | cd /src 30 | git clone git@github.com:bit-bots/bitbots_tf_buffer.git 31 | git clone git@github.com:bit-bots/ros2_python_extension.git 32 | ``` 33 | 34 | Then run rosdep to install the dependencies (most likely you already have them installed): 35 | 36 | ```bash 37 | rosdep update 38 | rosdep install --from-paths . --ignore-src -r -y 39 | ``` 40 | 41 | Now we can build and use the package: 42 | 43 | ```bash 44 | colcon build 45 | ``` 46 | -------------------------------------------------------------------------------- /bitbots_tf_buffer/__init__.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | 3 | import tf2_ros as tf2 4 | 5 | from rclpy.node import Node 6 | from builtin_interfaces.msg import Duration as DurationMsg 7 | from builtin_interfaces.msg import Time as TimeMsg 8 | from geometry_msgs.msg import TransformStamped 9 | from rclpy.duration import Duration 10 | from rclpy.serialization import deserialize_message, serialize_message 11 | from rclpy.time import Time 12 | 13 | from bitbots_tf_buffer.cpp_wrapper import Buffer as CppBuffer 14 | 15 | 16 | class Buffer(tf2.BufferCore, tf2.BufferInterface): 17 | """ 18 | Buffer class that wraps the C++ implementation of the tf2 buffer and listener. 19 | It spawns a new node with the suffix "_tf" to handle the C++ side of the ROS communication. 20 | """ 21 | 22 | def __init__(self, cache_time: Optional[Duration] = None, node: Optional[Node] = None): 23 | if cache_time is None: 24 | cache_time = Duration(seconds=10.0) 25 | 26 | tf2.BufferCore.__init__(self, cache_time) 27 | tf2.BufferInterface.__init__(self) 28 | 29 | self.cache_time = cache_time 30 | self._impl: Optional[CppBuffer] = None 31 | 32 | # If a node is provided, we can set the node directly 33 | if node is not None: 34 | self.set_node(node) 35 | 36 | def set_node(self, node: Node): 37 | """ 38 | This API is used instead of the constructor to set the node. 39 | This way we can have a dummy TransformListener and therefore 40 | keep compatibility with the official implementation. 41 | """ 42 | self._impl = CppBuffer(serialize_message(Duration.to_msg(self.cache_time)), node) 43 | 44 | def lookup_transform( 45 | self, target_frame: str, source_frame: str, time: Time | TimeMsg, timeout: Optional[Duration | DurationMsg] = None 46 | ) -> TransformStamped: 47 | assert self._impl is not None, "Buffer has not been initialized with a node. You either need to pass a node to the constructor or have a TransformListener set up." 48 | # Handle timeout as None 49 | timeout = timeout or Duration() 50 | # Call cpp implementation 51 | transform_str = self._impl.lookup_transform( 52 | target_frame, 53 | source_frame, 54 | serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)), 55 | serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)), 56 | ) 57 | return deserialize_message(transform_str, TransformStamped) 58 | 59 | def can_transform( 60 | self, target_frame: str, source_frame: str, time: Time | TimeMsg, timeout: Optional[Duration | DurationMsg] = None 61 | ) -> bool: 62 | assert self._impl is not None, "Buffer has not been initialized with a node. You either need to pass a node to the constructor or have a TransformListener set up." 63 | # Handle timeout as None 64 | timeout = timeout or Duration() 65 | # Call cpp implementation 66 | return self._impl.can_transform( 67 | target_frame, 68 | source_frame, 69 | serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)), 70 | serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)), 71 | ) 72 | 73 | class TransformListener: 74 | """ 75 | A dummy TransformListener that just sets the node into the C++ Buffer. 76 | This is done for compatibility with the original API implementation. 77 | """ 78 | def __init__( 79 | self, 80 | buffer: Buffer, 81 | node: Node, 82 | *ignored_args, 83 | **ignored_kwargs 84 | ) -> None: 85 | buffer.set_node(node) 86 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bitbots_tf_buffer 4 | 1.0.0 5 | A performant non blocking rclpy tf buffer utilizing rclcpp under the hood 6 | 7 | Florian Vahl 8 | 9 | MIT 10 | 11 | ament_cmake 12 | backward_ros 13 | builtin_interfaces 14 | geometry_msgs 15 | pybind11_vendor 16 | rcl 17 | rclcpp 18 | rclpy 19 | ros2_python_extension 20 | tf2_ros 21 | tf2 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name="bitbots_tf_buffer", 5 | packages=["bitbots_tf_buffer"], 6 | zip_safe=True, 7 | keywords=["ROS"], 8 | license="MIT", 9 | ) 10 | -------------------------------------------------------------------------------- /src/bitbots_tf_buffer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace py = pybind11; 19 | 20 | class Buffer { 21 | public: 22 | Buffer(py::bytes duration_raw, py::object node) { 23 | // initialize rclcpp if not already done 24 | if (!rclcpp::contexts::get_global_default_context()->is_valid()) { 25 | rclcpp::init(0, nullptr); 26 | } 27 | 28 | // Register the tf2 exceptions, so they can be caught in Python as expected 29 | auto py_tf2_ros = py::module::import("tf2_ros"); 30 | py::register_local_exception(py_tf2_ros, "LookupExceptionCpp", 31 | py_tf2_ros.attr("LookupException")); 32 | py::register_local_exception(py_tf2_ros, "ConnectivityExceptionCpp", 33 | py_tf2_ros.attr("ConnectivityException")); 34 | py::register_local_exception(py_tf2_ros, "ExtrapolationExceptionCpp", 35 | py_tf2_ros.attr("ExtrapolationException")); 36 | py::register_local_exception(py_tf2_ros, "InvalidArgumentExceptionCpp", 37 | py_tf2_ros.attr("InvalidArgumentException")); 38 | py::register_local_exception(py_tf2_ros, "TimeoutExceptionCpp", 39 | py_tf2_ros.attr("TimeoutException")); 40 | 41 | // get node name from python node object 42 | rcl_node_t *node_handle = 43 | static_cast(reinterpret_cast(node.attr("handle").attr("pointer").cast())); 44 | const char *node_name = rcl_node_get_name(node_handle); 45 | // create node with name _tf 46 | node_ = std::make_shared((std::string(node_name) + "_tf").c_str()); 47 | 48 | // Get buffer duration from python duration 49 | auto duration = 50 | tf2_ros::fromMsg(ros2_python_extension::fromPython(duration_raw)); 51 | 52 | // create subscribers 53 | buffer_ = std::make_shared(this->node_->get_clock(), duration); 54 | buffer_->setUsingDedicatedThread(true); 55 | listener_ = std::make_shared(*buffer_, node_, false); 56 | 57 | // create executor and start thread spinning the executor 58 | executor_ = std::make_shared(); 59 | executor_->add_node(node_); 60 | thread_ = std::make_shared([this]() { 61 | while (rclcpp::ok()) { 62 | executor_->spin_once(); 63 | } 64 | }); 65 | } 66 | 67 | py::bytes lookup_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) { 68 | // Convert python objects to C++ objects 69 | const std::string target_frame_str = target_frame.cast(); 70 | const std::string source_frame_str = source_frame.cast(); 71 | const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)}; 72 | const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)}; 73 | 74 | // Lookup transform 75 | auto transform = buffer_->lookupTransform(target_frame_str, source_frame_str, time_msg, timeout); 76 | 77 | // Convert C++ object back to python object 78 | return ros2_python_extension::toPython(transform); 79 | } 80 | 81 | bool can_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) { 82 | // Convert python objects to C++ objects 83 | const std::string target_frame_str = target_frame.cast(); 84 | const std::string source_frame_str = source_frame.cast(); 85 | const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)}; 86 | const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)}; 87 | // Check if transform can be looked up 88 | return buffer_->canTransform(target_frame_str, source_frame_str, time_msg, timeout); 89 | } 90 | 91 | // destructor 92 | ~Buffer() { 93 | // the executor finishes when rclcpp is shutdown, so the thread can be joined 94 | rclcpp::shutdown(); 95 | thread_->join(); 96 | } 97 | 98 | private: 99 | rclcpp::Serialization time_serializer_; 100 | rclcpp::Serialization duration_serializer_; 101 | rclcpp::Serialization transform_serializer_; 102 | 103 | std::shared_ptr node_; 104 | std::shared_ptr buffer_; 105 | std::shared_ptr listener_; 106 | std::shared_ptr thread_; 107 | std::shared_ptr executor_; 108 | }; 109 | 110 | PYBIND11_MODULE(cpp_wrapper, m) { 111 | py::class_>(m, "Buffer") 112 | .def(py::init()) 113 | .def("lookup_transform", &Buffer::lookup_transform) 114 | .def("can_transform", &Buffer::can_transform); 115 | } 116 | --------------------------------------------------------------------------------