From 554c8ff27593a6fe81d9a6f322bf5a33a54e63b1 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Mon, 20 Jan 2025 17:59:14 +0100 Subject: [PATCH] Use seperate repo for the tf_buffer --- bitbots_misc/bitbots_tf_buffer/CMakeLists.txt | 42 ------- bitbots_misc/bitbots_tf_buffer/README.md | 19 --- .../bitbots_tf_buffer/__init__.py | 59 --------- bitbots_misc/bitbots_tf_buffer/package.xml | 20 --- bitbots_misc/bitbots_tf_buffer/setup.py | 9 -- .../src/bitbots_tf_buffer.cpp | 114 ------------------ workspace.repos | 4 + 7 files changed, 4 insertions(+), 263 deletions(-) delete mode 100644 bitbots_misc/bitbots_tf_buffer/CMakeLists.txt delete mode 100644 bitbots_misc/bitbots_tf_buffer/README.md delete mode 100644 bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py delete mode 100644 bitbots_misc/bitbots_tf_buffer/package.xml delete mode 100644 bitbots_misc/bitbots_tf_buffer/setup.py delete mode 100644 bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp 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