diff --git a/README.md b/README.md index d6939c8c..d3ce7f3f 100644 --- a/README.md +++ b/README.md @@ -141,7 +141,7 @@ 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 ``` @@ -149,7 +149,7 @@ 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 diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index 3ba602f9..d96f0663 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/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, diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 0bc29567..c85254b0 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/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): diff --git a/free_fleet_examples/CMakeLists.txt b/free_fleet_examples/CMakeLists.txt index 39971358..44b75f6b 100644 --- a/free_fleet_examples/CMakeLists.txt +++ b/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} ) diff --git a/free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py b/free_fleet_examples/free_fleet_examples/cancel_all_goals.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py rename to free_fleet_examples/free_fleet_examples/cancel_all_goals.py diff --git a/free_fleet_examples/free_fleet_examples/tests/test_tf.py b/free_fleet_examples/free_fleet_examples/get_tf.py similarity index 64% rename from free_fleet_examples/free_fleet_examples/tests/test_tf.py rename to free_fleet_examples/free_fleet_examples/get_tf.py index 696dafc0..14f739bd 100755 --- a/free_fleet_examples/free_fleet_examples/tests/test_tf.py +++ b/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,6 +44,8 @@ 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 @@ -64,30 +53,20 @@ def main(argv=sys.argv): 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() diff --git a/free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py rename to free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index d9eb69b3..8bf93d3c 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/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