Merge pull request #20 from ros-sports/feature/fast_tf_buffer

Use fast tf_buffer if available

Author: Flova

.github/workflows/build_and_test_humble.yaml

@@ -6,11 +6,11 @@ name: Build and Test (humble)
 on:
   # Triggers the workflow on push
   push:
-    branches: [ rolling ]
+    branches: [ humble ]
 
   # Triggers the workflow on pull requests
   pull_request:
-    branches: [ rolling ]
+    branches: [ humble ]
 
   # Allows you to run this workflow manually from the Actions tab
   workflow_dispatch:

README.md

@@ -1,7 +1,10 @@
 # RoboCup Soccer Inverse Perspective Mapping
 
-[![Build and Test (humble)](../../actions/workflows/build_and_test_humble.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_humble.yaml?query=branch:rolling)
+[![Build and Test (humble)](../../actions/workflows/build_and_test_humble.yaml/badge.svg?branch=humble)](../../actions/workflows/build_and_test_humble.yaml?query=branch:humble)
 [![Build and Test (jazzy)](../../actions/workflows/build_and_test_jazzy.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_jazzy.yaml?query=branch:rolling)
 [![Build and Test (rolling)](../../actions/workflows/build_and_test_rolling.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_rolling.yaml?query=branch:rolling)
 
-This package is only available currently for ROS2 Rolling.
+
+## Tip
+
+Also install the [Bit-Bots TF Buffer](https:://github.com/bit-bots/bitbots_tf_buffer) to get a significant performance boost (up to a magnitude).

dependencies.repos

@@ -3,3 +3,11 @@ repositories:
     type: git
     url: https://github.com/ros-sports/ipm.git
     version: rolling
+  bitbots_tf_buffer:
+    type: git
+    url: https://github.com/bit-bots/bitbots_tf_buffer.git
+    version: main
+  ros2_python_extension:
+    type: git
+    url: https://github.com/bit-bots/ros2_python_extension.git
+    version: main

soccer_ipm/soccer_ipm/soccer_ipm.py

@@ -16,6 +16,7 @@
 import rclpy
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
+from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import CameraInfo
 from soccer_ipm.msgs.ball import map_ball_array
@@ -27,7 +28,13 @@
 from soccer_ipm.utils import catch_camera_info_not_set_error, catch_tf_timing_error
 import soccer_vision_2d_msgs.msg as sv2dm
 import soccer_vision_3d_msgs.msg as sv3dm
-import tf2_ros as tf2
+
+try:
+    from bitbots_tf_buffer import Buffer, TransformListener
+    fast_tf_buffer_available = True
+except ImportError:
+    from tf2_ros import Buffer, TransformListener
+    fast_tf_buffer_available = False
 
 
 class SoccerIPM(Node):
@@ -52,8 +59,8 @@ def __init__(self) -> None:
         self.declare_parameter('use_distortion', False)
 
         # We need to create a tf buffer
-        self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30.0))
-        self.tf_listener = tf2.TransformListener(self.tf_buffer, self)
+        self.tf_buffer = Buffer(cache_time=Duration(seconds=30.0))
+        self.tf_listener = TransformListener(self.tf_buffer, self)
 
         # Create an IPM instance
         self.ipm = IPM(self.tf_buffer, distortion=self.get_parameter('use_distortion').value)
@@ -218,7 +225,12 @@ def markings_sub(msg):
 def main(args=None):
     rclpy.init(args=args)
     node = SoccerIPM()
-    ex = MultiThreadedExecutor(num_threads=4)
+    # Due to the fact that the bitbots_tf_buffer handles all tf2 communication in another node
+    # we can use the single threaded EventsExecutor without running into deadlocks.
+    if fast_tf_buffer_available:
+        ex = EventsExecutor()
+    else:
+        ex = MultiThreadedExecutor(num_threads=4)
     ex.add_node(node)
     try:
         ex.spin()