diff --git a/bitbots_misc/bitbots_tf_buffer/CMakeLists.txt b/bitbots_misc/bitbots_tf_buffer/CMakeLists.txt
deleted file mode 100644
index 501ca675d..000000000
--- a/bitbots_misc/bitbots_tf_buffer/CMakeLists.txt
+++ /dev/null
@@ -1,42 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(bitbots_tf_buffer)
-
-# Add support for C++17
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
-endif()
-
-set(PYBIND11_PYTHON_VERSION 3)
-set(PYBIND11_FINDPYTHON ON)
-find_package(ament_cmake REQUIRED)
-find_package(backward_ros REQUIRED)
-find_package(pybind11 REQUIRED)
-find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
-find_package(rcl REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(ros2_python_extension REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
-
-add_compile_options(-Wall -Wno-unused)
-
-pybind11_add_module(cpp_wrapper SHARED src/bitbots_tf_buffer.cpp)
-
-ament_target_dependencies(
- cpp_wrapper
- PUBLIC
- rclcpp
- rcl
- tf2
- tf2_ros
- ros2_python_extension)
-
-target_link_libraries(cpp_wrapper PRIVATE pybind11::module)
-
-ament_python_install_package(${PROJECT_NAME})
-
-ament_get_python_install_dir(PYTHON_INSTALL_DIR)
-
-install(TARGETS cpp_wrapper DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}")
-
-ament_package()
diff --git a/bitbots_misc/bitbots_tf_buffer/README.md b/bitbots_misc/bitbots_tf_buffer/README.md
deleted file mode 100644
index c9d7e4bf1..000000000
--- a/bitbots_misc/bitbots_tf_buffer/README.md
+++ /dev/null
@@ -1,19 +0,0 @@
-# bitbots_tf_buffer
-
-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`, except that we need to pass a reference to the node to the constructor.
-
-## Usage
-
-- Replace `from tf2_ros import Buffer, TransformListener` with `from bitbots_tf_buffer import
-Buffer`.
-- Remove the `TransformListener` from the code
-- Pass a reference to the node to the constructor of `Buffer`:
-
-```python
-from bitbots_tf_buffer import Buffer
-
-class MyNode(Node):
- def __init__(self):
- super().__init__('my_node')
- self.tf_buffer = Buffer(self)
-```
diff --git a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py b/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py
deleted file mode 100644
index cf824a0b6..000000000
--- a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py
+++ /dev/null
@@ -1,59 +0,0 @@
-from typing import Optional
-
-import tf2_ros as tf2
-
-# Setting up runtime type checking for this package
-from beartype.claw import beartype_this_package
-from builtin_interfaces.msg import Duration as DurationMsg
-from builtin_interfaces.msg import Time as TimeMsg
-from geometry_msgs.msg import TransformStamped
-from rclpy.duration import Duration
-from rclpy.serialization import deserialize_message, serialize_message
-from rclpy.time import Time
-
-from bitbots_tf_buffer.cpp_wrapper import Buffer as CppBuffer
-
-beartype_this_package()
-
-
-class Buffer(tf2.BufferCore, tf2.BufferInterface):
- """
- Buffer class that wraps the C++ implementation of the tf2 buffer and listener.
- It spawns a new node with the suffix "_tf" to handle the C++ side of the ROS communication.
- """
-
- def __init__(self, node, cache_time: Optional[Duration] = None, *args, **kwargs):
- if cache_time is None:
- cache_time = Duration(seconds=10.0)
-
- tf2.BufferCore.__init__(self, cache_time)
- tf2.BufferInterface.__init__(self)
-
- self._impl = CppBuffer(serialize_message(Duration.to_msg(cache_time)), node)
-
- def lookup_transform(
- self, target_frame: str, source_frame: str, time: Time, timeout: Optional[Duration] = None
- ) -> TransformStamped:
- # Handle timeout as None
- timeout = timeout or Duration()
- # Call cpp implementation
- transform_str = self._impl.lookup_transform(
- target_frame,
- source_frame,
- serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)),
- serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)),
- )
- return deserialize_message(transform_str, TransformStamped)
-
- def can_transform(
- self, target_frame: str, source_frame: str, time: Time, timeout: Optional[Duration] = None
- ) -> bool:
- # Handle timeout as None
- timeout = timeout or Duration()
- # Call cpp implementation
- return self._impl.can_transform(
- target_frame,
- source_frame,
- serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)),
- serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)),
- )
diff --git a/bitbots_misc/bitbots_tf_buffer/package.xml b/bitbots_misc/bitbots_tf_buffer/package.xml
deleted file mode 100644
index 941051664..000000000
--- a/bitbots_misc/bitbots_tf_buffer/package.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-
-
- bitbots_tf_buffer
- 1.0.0
- Drop-in replacement for tf listener to improve performance
-
- Timon Engelke
-
- MIT
-
- ament_cmake
- pybind11_vendor
- ros2_python_extension
- tf2_ros
- tf2
-
-
- ament_cmake
-
-
diff --git a/bitbots_misc/bitbots_tf_buffer/setup.py b/bitbots_misc/bitbots_tf_buffer/setup.py
deleted file mode 100644
index 0c4c1d8ee..000000000
--- a/bitbots_misc/bitbots_tf_buffer/setup.py
+++ /dev/null
@@ -1,9 +0,0 @@
-from setuptools import setup
-
-setup(
- name="bitbots_tf_buffer",
- packages=["bitbots_tf_buffer"],
- zip_safe=True,
- keywords=["ROS"],
- license="MIT",
-)
diff --git a/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp b/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp
deleted file mode 100644
index 1096bf9a8..000000000
--- a/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace py = pybind11;
-
-class Buffer {
- public:
- Buffer(py::bytes duration_raw, py::object node) {
- // initialize rclcpp if not already done
- if (!rclcpp::contexts::get_global_default_context()->is_valid()) {
- rclcpp::init(0, nullptr);
- }
-
- // Register the tf2 exceptions, so they can be caught in Python as expected
- auto py_tf2_ros = py::module::import("tf2_ros");
- py::register_local_exception(py_tf2_ros, "LookupExceptionCpp",
- py_tf2_ros.attr("LookupException"));
- py::register_local_exception(py_tf2_ros, "ConnectivityExceptionCpp",
- py_tf2_ros.attr("ConnectivityException"));
- py::register_local_exception(py_tf2_ros, "ExtrapolationExceptionCpp",
- py_tf2_ros.attr("ExtrapolationException"));
- py::register_local_exception(py_tf2_ros, "InvalidArgumentExceptionCpp",
- py_tf2_ros.attr("InvalidArgumentException"));
- py::register_local_exception(py_tf2_ros, "TimeoutExceptionCpp",
- py_tf2_ros.attr("TimeoutException"));
-
- // get node name from python node object
- rcl_node_t *node_handle = (rcl_node_t *)node.attr("handle").attr("pointer").cast();
- const char *node_name = rcl_node_get_name(node_handle);
- // create node with name _tf
- node_ = std::make_shared((std::string(node_name) + "_tf").c_str());
-
- // Get buffer duration from python duration
- auto duration =
- tf2_ros::fromMsg(ros2_python_extension::fromPython(duration_raw));
-
- // create subscribers
- buffer_ = std::make_shared(this->node_->get_clock(), duration);
- buffer_->setUsingDedicatedThread(true);
- listener_ = std::make_shared(*buffer_, node_, false);
-
- // create executor and start thread spinning the executor
- executor_ = std::make_shared();
- executor_->add_node(node_);
- thread_ = std::make_shared([this]() {
- while (rclcpp::ok()) {
- executor_->spin_once();
- }
- });
- }
-
- py::bytes lookup_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) {
- // Convert python objects to C++ objects
- const std::string target_frame_str = target_frame.cast();
- const std::string source_frame_str = source_frame.cast();
- const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)};
- const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)};
-
- // Lookup transform
- auto transform = buffer_->lookupTransform(target_frame_str, source_frame_str, time_msg, timeout);
-
- // Convert C++ object back to python object
- return ros2_python_extension::toPython(transform);
- }
-
- bool can_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) {
- // Convert python objects to C++ objects
- const std::string target_frame_str = target_frame.cast();
- const std::string source_frame_str = source_frame.cast();
- const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)};
- const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)};
- // Check if transform can be looked up
- return buffer_->canTransform(target_frame_str, source_frame_str, time_msg, timeout);
- }
-
- // destructor
- ~Buffer() {
- // the executor finishes when rclcpp is shutdown, so the thread can be joined
- rclcpp::shutdown();
- thread_->join();
- }
-
- private:
- rclcpp::Serialization time_serializer_;
- rclcpp::Serialization duration_serializer_;
- rclcpp::Serialization transform_serializer_;
-
- std::shared_ptr node_;
- std::shared_ptr buffer_;
- std::shared_ptr listener_;
- std::shared_ptr thread_;
- std::shared_ptr executor_;
-};
-
-PYBIND11_MODULE(cpp_wrapper, m) {
- py::class_>(m, "Buffer")
- .def(py::init())
- .def("lookup_transform", &Buffer::lookup_transform)
- .def("can_transform", &Buffer::can_transform);
-}
diff --git a/workspace.repos b/workspace.repos
index 22222f1bb..736f21ff3 100644
--- a/workspace.repos
+++ b/workspace.repos
@@ -3,6 +3,10 @@ repositories:
type: git
url: git@github.com:bit-bots/RobocupProtocol.git
version: master
+ lib/bitbots_tf_buffer:
+ type: git
+ url: git@github.com:bit-bots/bitbots_tf_buffer.git
+ version: iron
lib/dynamic_stack_decider:
type: git
url: git@github.com:bit-bots/dynamic_stack_decider.git