#!/usr/bin/env python3 """Deterministic ROS 2 Jazzy transport exercise; no robot or localization input. Run through verify.sh in the documented isolated container. Three nodes exchange Odometry, TFMessage and static TFMessage data through ROS publishers/subscribers. The synthetic stamps 10, 11 and 12 are dataset times, not this machine's clock. """ import math import time from collections.abc import Callable import rclpy import yaml from geometry_msgs.msg import PointStamped, TransformStamped from nav_msgs.msg import Odometry from rclpy.duration import Duration from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.time import Time from tf2_ros import ( Buffer, ConnectivityException, ExtrapolationException, LookupException, StaticTransformBroadcaster, TransformBroadcaster, TransformListener, ) import tf2_geometry_msgs # noqa: F401: registers PointStamped with Buffer.transform. def require(condition: bool, message: str) -> None: """Use explicit failures so checks also run under python -O.""" if not condition: raise RuntimeError(message) def wait_until(executor: SingleThreadedExecutor, predicate: Callable[[], bool], description: str, timeout: float = 10.0) -> None: """Keep ROS callbacks running while waiting for transport, with a wall deadline.""" deadline = time.monotonic() + timeout while not predicate(): if time.monotonic() >= deadline: raise RuntimeError(f"Transport timed out: {description}") executor.spin_once(timeout_sec=0.05) def static_transform(parent: str, child: str, x: float, y: float) -> TransformStamped: transform = TransformStamped() transform.header.frame_id = parent transform.child_frame_id = child # /tf_static describes a time-invariant relationship, so this zero stamp is fine. transform.transform.translation.x = x transform.transform.translation.y = y transform.transform.rotation.w = 1.0 return transform def odometry_sample(stamp: int, x: float) -> Odometry: message = Odometry() message.header.stamp = Time(seconds=stamp).to_msg() message.header.frame_id = "odom" message.child_frame_id = "base_link" message.pose.pose.position.x = x message.pose.pose.position.y = 2.0 message.pose.pose.orientation.z = math.sin(math.pi / 4.0) message.pose.pose.orientation.w = math.cos(math.pi / 4.0) # Moving along odom +x at 1 m/s, with base +x along odom +y, is base -y. message.twist.twist.linear.y = -1.0 # Covariances remain zero placeholders for these exact synthetic poses. # This example neither estimates uncertainty nor transfers it into TF. return message class OdometryBridge(Node): def __init__(self) -> None: super().__init__("lesson_odometry_bridge") self.enabled = False self.received = 0 self.broadcast = 0 self.broadcaster = TransformBroadcaster(self) self.static_broadcaster = StaticTransformBroadcaster(self) self.subscription = self.create_subscription(Odometry, "/odom", self.on_odometry, 10) def on_odometry(self, message: Odometry) -> None: self.received += 1 if not self.enabled: return # This narrowly scoped bridge accepts only this exercise's known frame pair. require(message.header.frame_id == "odom", "Unexpected odometry parent") require(message.child_frame_id == "base_link", "Unexpected odometry child") transform = TransformStamped() transform.header = message.header # Preserve measurement time; do not use now(). transform.child_frame_id = message.child_frame_id transform.transform.translation.x = message.pose.pose.position.x transform.transform.translation.y = message.pose.pose.position.y transform.transform.translation.z = message.pose.pose.position.z transform.transform.rotation = message.pose.pose.orientation self.broadcaster.sendTransform(transform) self.broadcast += 1 def point_in_laser(stamp: int) -> PointStamped: point = PointStamped() point.header.frame_id = "laser" point.header.stamp = Time(seconds=stamp).to_msg() point.point.x = 1.0 return point def check_point(point: PointStamped, frame: str, stamp: int, expected: tuple[float, float, float]) -> None: require(point.header.frame_id == frame, f"Expected output frame {frame}") require(Time.from_msg(point.header.stamp).nanoseconds == stamp * 1_000_000_000, f"Expected output stamp {stamp}") actual = (point.point.x, point.point.y, point.point.z) require(all(math.isclose(a, b, rel_tol=0.0, abs_tol=1e-9) for a, b in zip(actual, expected)), f"Expected {expected}, got {actual}") def expect_exception(expected: type[Exception], operation: Callable[[], object], description: str) -> None: try: operation() except expected: print(f"{description}: {expected.__name__}") else: raise RuntimeError(f"Expected {expected.__name__}: {description}") def run() -> None: rclpy.init() source = Node("lesson_odometry_source") bridge = OdometryBridge() observer = Node("lesson_tf_observer") publisher = source.create_publisher(Odometry, "/odom", 10) buffer = Buffer(cache_time=Duration(seconds=10.0)) listener = TransformListener(buffer, observer, spin_thread=False) executor = SingleThreadedExecutor() for node in (source, bridge, observer): executor.add_node(node) try: # All transport is local to the container. Await matching instead of sleeping # and hoping that a volatile dynamic transform has reached a subscriber. wait_until(executor, lambda: publisher.get_subscription_count() == 1, "the /odom subscription") wait_until(executor, lambda: bridge.broadcaster.pub_tf.get_subscription_count() == 1, "the /tf subscription") bridge.static_broadcaster.sendTransform([ static_transform("map", "odom", 10.0, -1.0), static_transform("base_link", "laser", 0.2, 0.0), ]) wait_until(executor, lambda: buffer.can_transform("map", "odom", Time()) and buffer.can_transform("base_link", "laser", Time()), "both static transforms") samples = (odometry_sample(10, 1.0), odometry_sample(12, 3.0)) for message in samples: publisher.publish(message) wait_until(executor, lambda: bridge.received == 2, "two /odom samples") require(bridge.broadcast == 0, "Disabled bridge unexpectedly broadcast TF") require(not buffer.can_transform("map", "laser", Time(seconds=11)), "/odom unexpectedly supplied the missing TF edge") print("ODOMETRY ONLY: two /odom samples received; map <- laser unavailable") expect_exception(ConnectivityException, lambda: buffer.lookup_transform("map", "laser", Time(seconds=11)), "DISCONNECTED TREE") bridge.enabled = True for message in samples: publisher.publish(message) wait_until(executor, lambda: bridge.broadcast == 2 and buffer.can_transform("map", "laser", Time(seconds=10)) and buffer.can_transform("map", "laser", Time(seconds=12)), "the explicit bridge and both dynamic samples") print("BRIDGE ENABLED: /odom callback published odom -> base_link at t=10 and t=12") frames = yaml.safe_load(buffer.all_frames_as_yaml()) for child, parent in (("odom", "map"), ("base_link", "odom"), ("laser", "base_link")): require(frames[child]["parent"] == parent, f"Unexpected parent for {child}") print("BUFFER TREE: map -> odom -> base_link -> laser") transform_at_11 = buffer.lookup_transform("map", "laser", Time(seconds=11)) translation = transform_at_11.transform.translation require(math.isclose(translation.x, 12.0, rel_tol=0.0, abs_tol=1e-9) and math.isclose(translation.y, 1.2, rel_tol=0.0, abs_tol=1e-9) and math.isclose(translation.z, 0.0, rel_tol=0.0, abs_tol=1e-9), "Expected map <- laser translation (12, 1.2, 0) at t=11") rotation = transform_at_11.transform.rotation require(math.isclose(rotation.x, 0.0, rel_tol=0.0, abs_tol=1e-9) and math.isclose(rotation.y, 0.0, rel_tol=0.0, abs_tol=1e-9) and math.isclose(rotation.z, math.sqrt(0.5), rel_tol=0.0, abs_tol=1e-9) and math.isclose(rotation.w, math.sqrt(0.5), rel_tol=0.0, abs_tol=1e-9), "Expected map <- laser yaw +90 degrees at t=11") print("TRANSFORM t=11: map <- laser translation (12.000, 1.200, 0.000), yaw +90 deg") # Buffer.transform uses the source message's stamp and registered point adapter. stamped_result = buffer.transform(point_in_laser(11), "map") check_point(stamped_result, "map", 11, (12.0, 2.2, 0.0)) print("STAMPED t=11: laser (1.000, 0.000, 0.000) -> map (12.000, 2.200, 0.000)") recovered = buffer.transform(stamped_result, "laser") check_point(recovered, "laser", 11, (1.0, 0.0, 0.0)) print("INVERSE t=11: map (12.000, 2.200, 0.000) -> laser (1.000, 0.000, 0.000)") # This intentionally substitutes the wrong time for the old measurement. # Zero requests the latest available common time, not acquisition time. latest_result = buffer.transform(point_in_laser(0), "map") check_point(latest_result, "map", 12, (13.0, 2.2, 0.0)) error = math.dist((stamped_result.point.x, stamped_result.point.y, stamped_result.point.z), (latest_result.point.x, latest_result.point.y, latest_result.point.z)) require(math.isclose(error, 1.0, rel_tol=0.0, abs_tol=1e-9), "Expected a 1 m time-substitution error") print("LATEST t=0: resolved t=12 -> map (13.000, 2.200, 0.000); difference = 1.000 m") expect_exception(LookupException, lambda: buffer.lookup_transform("map", "missing_sensor", Time(seconds=11)), "UNKNOWN FRAME") for stamp, label in ((9, "PAST t=9"), (13, "FUTURE t=13")): expect_exception(ExtrapolationException, lambda stamp=stamp: buffer.lookup_transform("map", "laser", Time(seconds=stamp)), label) print("PASS: ROS transport, explicit bridge, interpolation, inverse, latest time, and error checks") finally: listener.unregister() executor.shutdown() for node in (source, bridge, observer): node.destroy_node() rclpy.shutdown() if __name__ == "__main__": run()