Refactor to TfHandler, added testing

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

README.md

@@ -141,15 +141,15 @@ Listen to transforms over `zenoh`,
 
 ```bash
 source ~/ff_ws/install/setup.bash
-ros2 run free_fleet_examples test_tf.py \
+ros2 run free_fleet_examples get_tf.py \
     --namespace turtlebot3_1
 ```
 
 Start a `navigate_to_pose` action over `zenoh`, using example values,
 
 ```bash
 source ~/ff_ws/install/setup.bash
-ros2 run free_fleet_examples test_navigate_to_pose.py \
+ros2 run free_fleet_examples send_navigate_to_pose.py \
     --frame-id map \
     --namespace turtlebot3_1 \
     -x 1.808 \
@@ -294,7 +294,7 @@ ros2 run rmf_demos_tasks dispatch_patrol \
 
 * hardware testing
 * attempt to optimize tf messages (not all are needed)
-* robot adapter to be abstracted
+* robot adapter and tf handler to be abstracted
 * ROS 1 nav support
 * custom actions to be abstracted
 * map switching support

free_fleet_adapter/free_fleet_adapter/fleet_adapter.py

@@ -20,8 +20,6 @@
 import threading
 import time
 
-from free_fleet.utils import namespacify
-
 import nudged
 import rclpy
 from rclpy.duration import Duration
@@ -214,25 +212,21 @@ def run_in_parallel(*args, **kwargs):
 
 @parallel
 def update_robot(robot: Nav2RobotAdapter, tf_buffer: Buffer):
-    try:
-        # TODO(ac): parameterize the frames for lookup
-        transform = tf_buffer.lookup_transform(
-            namespacify('map', robot.name),
-            namespacify('base_footprint', robot.name),
-            rclpy.time.Time()
-        )
-        orientation = euler_from_quaternion([
-            transform.transform.rotation.x,
-            transform.transform.rotation.y,
-            transform.transform.rotation.z,
-            transform.transform.rotation.w
-        ])
-    except Exception as err:
+    transform = robot.tf_handler.get_transform()
+    if transform is None:
         robot.node.get_logger().info(
             f'Failed to update robot [{robot.name}]: Unable to get transform '
-            f'between base_footprint and map: {type(err)}: {err}'
+            f'between base_footprint and map'
         )
         return None
+
+    orientation = euler_from_quaternion([
+        transform.transform.rotation.x,
+        transform.transform.rotation.y,
+        transform.transform.rotation.z,
+        transform.transform.rotation.w
+    ])
+
     robot_pose = [
         transform.transform.translation.x,
         transform.transform.translation.y,

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

@@ -36,33 +36,39 @@
     namespacify,
 )
 
+from geometry_msgs.msg import TransformStamped
 import numpy as np
+import rclpy
 import rmf_adapter.easy_full_control as rmf_easy
 from tf_transformations import quaternion_from_euler
 import zenoh
 
 
-class TfListener:
+class TfHandler:
 
-    def __init__(self, robot_name, zenoh_session, tf_buffer):
+    def __init__(self, robot_name, zenoh_session, tf_buffer, node=None):
         self.robot_name = robot_name
         self.zenoh_session = zenoh_session
+        self.node = node
         self.tf_buffer = tf_buffer
 
         def _tf_callback(sample: zenoh.Sample):
             try:
                 transform = TFMessage.deserialize(sample.payload.to_bytes())
             except Exception as e:
-                self.node.get_logger().debug(
+                error_message = \
                     f'Failed to deserialize TF payload: {type(e)}: {e}'
-                )
+                if self.node is not None:
+                    self.node.get_logger().debug(error_message)
+                else:
+                    print(error_message)
                 return None
             for zt in transform.transforms:
                 t = transform_stamped_to_ros2_msg(zt)
                 t.header.frame_id = namespacify(zt.header.frame_id,
-                                                self.name)
+                                                self.robot_name)
                 t.child_frame_id = namespacify(zt.child_frame_id,
-                                               self.name)
+                                               self.robot_name)
                 self.tf_buffer.set_transform(
                     t, f'{self.robot_name}_TfListener')
 
@@ -71,6 +77,25 @@ def _tf_callback(sample: zenoh.Sample):
             _tf_callback
         )
 
+    def get_transform(self) -> TransformStamped | None:
+        try:
+            # TODO(ac): parameterize the frames for lookup
+            transform = self.tf_buffer.lookup_transform(
+                namespacify('map', self.robot_name),
+                namespacify('base_footprint', self.robot_name),
+                rclpy.time.Time()
+            )
+            return transform
+        except Exception as err:
+            error_message = \
+                'Unable to get transform between base_footprint and map: ' \
+                f'{type(err)}: {err}'
+            if self.node is not None:
+                self.node.get_logger().info(error_message)
+            else:
+                print(error_message)
+        return None
+
 
 class Nav2RobotAdapter:
 
@@ -102,25 +127,11 @@ def __init__(
 
         self.replan_counts = 0
 
-        def _tf_callback(sample: zenoh.Sample):
-            try:
-                transform = TFMessage.deserialize(sample.payload.to_bytes())
-            except Exception as e:
-                self.node.get_logger().debug(
-                    f'Failed to deserialize TF payload: {type(e)}: {e}'
-                )
-                return None
-            for zt in transform.transforms:
-                t = transform_stamped_to_ros2_msg(zt)
-                t.header.frame_id = namespacify(zt.header.frame_id,
-                                                self.name)
-                t.child_frame_id = namespacify(zt.child_frame_id,
-                                               self.name)
-                self.tf_buffer.set_transform(t, f'{self.name}_RobotAdapter')
-
-        self.tf_sub = self.zenoh_session.declare_subscriber(
-            namespacify('tf', self.name),
-            _tf_callback
+        self.tf_handler = TfHandler(
+            self.name,
+            self.zenoh_session,
+            self.tf_buffer,
+            self.node
         )
 
         def _battery_state_callback(sample: zenoh.Sample):

free_fleet_examples/CMakeLists.txt

@@ -47,9 +47,9 @@ foreach(path ${traffic_editor_paths})
 endforeach()
 
 install(PROGRAMS
-  ${PROJECT_NAME}/tests/test_cancel_all_goals.py
-  ${PROJECT_NAME}/tests/test_navigate_to_pose.py
-  ${PROJECT_NAME}/tests/test_tf.py
+  ${PROJECT_NAME}/cancel_all_goals.py
+  ${PROJECT_NAME}/get_tf.py
+  ${PROJECT_NAME}/send_navigate_to_pose.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

free_fleet_examples/free_fleet_examples/tests/test_tf.py → free_fleet_examples/free_fleet_examples/get_tf.py

@@ -18,28 +18,15 @@
 import sys
 import time
 
-from free_fleet.convert import transform_stamped_to_ros2_msg
-from free_fleet.types import TFMessage
-from free_fleet.utils import namespacify
-from rclpy.time import Time
+from free_fleet_adapter.nav2_robot_adapter import TfHandler
 from tf2_ros import Buffer
 
 import zenoh
 
 
-tf_buffer = Buffer()
-
-
-def tf_callback(sample: zenoh.Sample):
-    transform = TFMessage.deserialize(sample.payload.to_bytes())
-    for zt in transform.transforms:
-        t = transform_stamped_to_ros2_msg(zt)
-        tf_buffer.set_transform(t, 'free_fleet_examples_test_tf')
-
-
 def main(argv=sys.argv):
     parser = argparse.ArgumentParser(
-        prog='tf_listener',
+        prog='get_tf',
         description='Zenoh/ROS2 tf example')
     parser.add_argument('--zenoh-config', '-c', dest='config', metavar='FILE',
                         type=str, help='A configuration file.')
@@ -57,37 +44,29 @@ def main(argv=sys.argv):
 
     zenoh.try_init_log_from_env()
 
+    tf_buffer = Buffer()
+
     # Open Zenoh Session
     with zenoh.open(conf) as session:
         info = session.info
         print(f'zid: {info.zid()}')
         print(f'routers: {info.routers_zid()}')
         print(f'peers: {info.peers_zid()}')
 
-        # Subscribe to TF
-        pub = session.declare_subscriber(
-            namespacify('tf', args.namespace),
-            tf_callback
-        )
+        tf_handler = TfHandler('turtlebot3_1', session, tf_buffer)
 
         try:
             while True:
-                try:
-                    transform = tf_buffer.lookup_transform(
-                        args.base_footprint_frame,
-                        args.map_frame,
-                        Time()
-                    )
+                transform = tf_handler.get_transform()
+                if transform is None:
+                    print('Unable to get transform between base_footprint and'
+                          ' map')
+                else:
                     print(transform)
-                except Exception as err:
-                    print(f'Unable to get transform between base_footprint and'
-                          f' map: {type(err)}: {err}')
-
                 time.sleep(1)
         except (KeyboardInterrupt):
             pass
         finally:
-            pub.undeclare()
             session.close()
 
 

free_fleet_examples/tests/integration/test_tf.py

@@ -16,35 +16,43 @@
 
 import time
 
-from free_fleet_adapter.nav2_robot_adapter import TfListener
-from rclpy.time import Time
+from free_fleet_adapter.nav2_robot_adapter import TfHandler
 from tf2_ros import Buffer
 
 import zenoh
 
 
-def test_tf():
+def test_tf_does_not_exist():
     zenoh.try_init_log_from_env()
     with zenoh.open(zenoh.Config()) as session:
         tf_buffer = Buffer()
 
-        listener = TfListener('turtlebot3_1', session, tf_buffer)
-        listener
+        tf_handler = TfHandler('missing_turtlebot3_1', session, tf_buffer)
 
         transform_exists = False
         for i in range(10):
-            try:
-                tf_buffer.lookup_transform(
-                    'turtlebot3_1/base_footprint',
-                    'turtlebot3_1/map',
-                    Time()
-                )
+            transform = tf_handler.get_transform()
+            if transform is not None:
                 transform_exists = True
                 break
-            except Exception as err:
-                print(f'Unable to get transform between base_footprint and '
-                      f'map: {type(err)}: {err}')
+            time.sleep(1)
+
+        assert not transform_exists
+
+
+def test_tf_exists():
+    zenoh.try_init_log_from_env()
+    with zenoh.open(zenoh.Config()) as session:
+        tf_buffer = Buffer()
+
+        tf_handler = TfHandler('turtlebot3_1', session, tf_buffer)
 
+        transform_exists = False
+        for i in range(10):
+            transform = tf_handler.get_transform()
+            if transform is not None:
+                transform_exists = True
+                break
             time.sleep(1)
 
         assert transform_exists