Python TransformFrame

from horus import Transform

# Identity transform
tf = Transform.identity()

# From translation and rotation quaternion
tf = Transform(translation=[1.0, 2.0, 0.0], rotation=[0.0, 0.0, 0.0, 1.0])

# From translation only (identity rotation)
tf = Transform.from_translation([1.0, 2.0, 3.0])

# From Euler angles (translation + roll/pitch/yaw)
tf = Transform.from_euler([1.0, 0.0, 0.5], [0.0, 0.1, 1.57])

# From 4x4 homogeneous matrix
tf = Transform.from_matrix(matrix_4x4)

tf.translation  # [x, y, z] as list of floats
tf.rotation     # [x, y, z, w] quaternion as list of floats

tf.translation = [2.0, 3.0, 0.0]
tf.rotation = [0.0, 0.0, 0.383, 0.924]  # 45 degrees around Z

# Convert to Euler angles
roll, pitch, yaw = tf.to_euler()

# Compose transforms: result = self * other
combined = parent_tf.compose(child_tf)

# Inverse transform
inv = tf.inverse()

# Apply to a point
point_in_world = tf.transform_point([1.0, 0.0, 0.0])

# Apply rotation only (no translation)
rotated = tf.transform_vector([1.0, 0.0, 0.0])

# Interpolate between two transforms (SLERP for rotation)
halfway = tf_a.interpolate(tf_b, t=0.5)

# Magnitudes
dist = tf.translation_magnitude()  # Translation distance
angle = tf.rotation_angle()        # Rotation angle in radians

# Export as 4x4 matrix
matrix = tf.to_matrix()  # [[f64; 4]; 4]

from horus import TransformFrame, TransformFrameConfig

# Default configuration
tf_tree = TransformFrame()

# With custom config
config = TransformFrameConfig(max_frames=1024, history_len=64)
tf_tree = TransformFrame(config=config)

# Preset sizes
tf_tree = TransformFrame.small()    # 256 frames, ~550KB
tf_tree = TransformFrame.medium()   # 1024 frames, ~2.2MB
tf_tree = TransformFrame.large()    # 4096 frames, ~9MB
tf_tree = TransformFrame.massive()  # 16384 frames, ~35MB

# Register child frame under a parent — returns frame ID (int)
frame_id = tf_tree.register_frame("base_link", "world")
tf_tree.register_frame("lidar", "base_link")
tf_tree.register_frame("camera", "base_link")

# Unregister a frame (raises HorusNotFoundError if not found)
tf_tree.unregister_frame("camera")

from horus import Transform

# Set the transform from parent to child (raises on unknown frame)
tf_tree.update_transform("base_link", Transform(
    translation=[1.0, 0.0, 0.0],
    rotation=[0.0, 0.0, 0.0, 1.0]
))

# With optional explicit timestamp
tf_tree.update_transform("lidar", Transform.from_translation([0.0, 0.0, 0.3]),
                         timestamp_ns=1234567890)

# Get transform from source frame to target frame
tf = tf_tree.tf("lidar", "world")
print(f"Lidar in world: {tf.translation}")

# With specific timestamp (for interpolation)
tf = tf_tree.tf_at("lidar", "world", timestamp_ns=1234567890)

# List all registered frames
frames = tf_tree.all_frames()  # ["world", "base_link", "lidar", "camera"]

# Get parent of a frame
parent = tf_tree.parent("lidar")  # "base_link"

# Get children of a frame
children = tf_tree.children("base_link")  # ["lidar", "camera"]

# Seconds since last transform update (None if never updated)
age = tf_tree.time_since_last_update("lidar")  # e.g., 0.015

# Wait for a transform to become available (blocking with timeout)
tf = tf_tree.wait_for_transform("lidar", "world", timeout_sec=1.0)
print(f"Got transform: {tf.translation}")

# Static frames — set once, never changes
tf_tree.register_static_frame("lidar", Transform.from_translation([0.0, 0.0, 0.3]), parent="base_link")

# Transform points directly between frames
world_point = tf_tree.transform_point("lidar", "world", [5.0, 0.0, 0.0])

# Async wait (non-blocking)
import concurrent.futures
future = tf_tree.wait_for_transform_async("lidar", "world", timeout_sec=2.0)
tf = future.result()  # Blocks until ready

# Check staleness before using data
if tf_tree.is_stale("base_link", max_age_sec=0.1):
    print("Odometry data is stale!")

# Diagnostics
print(tf_tree.format_tree())   # Visual tree
print(tf_tree.stats())         # {"frames": 5, "lookups": 1234, ...}
tf_tree.validate()             # Checks tree integrity

from horus import TransformFrameConfig

# Custom
config = TransformFrameConfig(max_frames=512, history_len=16)

# Presets
config = TransformFrameConfig.small()    # 256 frames, ~550KB
config = TransformFrameConfig.medium()   # 1024 frames, ~2.2MB
config = TransformFrameConfig.large()    # 4096 frames, ~9MB
config = TransformFrameConfig.massive()  # 16384 frames, ~35MB

# Check memory usage
print(config.max_frames)       # 512
print(config.history_len)      # 16
print(config.memory_estimate())  # "~1.1MB"

from horus import Node, Scheduler, TransformFrame, Transform
import math

# Global transform tree
tf_tree = TransformFrame.medium()

def setup_frames(node):
    tf_tree.register_frame("base_link", "world")
    tf_tree.register_frame("lidar", "base_link")
    tf_tree.register_frame("camera", "base_link")

    # Static transform: lidar is 30cm above base
    tf_tree.update_transform("lidar", Transform.from_translation([0.0, 0.0, 0.3]))

    # Static transform: camera is 10cm forward, 15cm up
    tf_tree.update_transform("camera", Transform.from_translation([0.1, 0.0, 0.15]))

tick_count = 0

def odometry_tick(node):
    global tick_count
    tick_count += 1

    # Simulate robot moving in a circle
    t = tick_count * 0.01
    x = math.cos(t) * 2.0
    y = math.sin(t) * 2.0
    yaw = t + math.pi / 2

    # Update base_link in world
    tf_tree.update_transform("base_link",
        Transform.from_euler([x, y, 0.0], [0.0, 0.0, yaw]))

def perception_tick(node):
    # Transform a lidar point into world coordinates
    try:
        lidar_to_world = tf_tree.tf("lidar", "world")
        point_in_world = lidar_to_world.transform_point([5.0, 0.0, 0.0])
        node.log_info(f"Obstacle at world: {point_in_world}")
    except Exception:
        pass  # Frame not yet available

odom = Node(name="odom", tick=odometry_tick, init=setup_frames, rate=100)
percept = Node(name="perception", tick=perception_tick, rate=10)

scheduler = Scheduler()
scheduler.add(odom, order=0)
scheduler.add(percept, order=1)
scheduler.run(duration=10)

from horus import get_timestamp_ns

# Get current time in nanoseconds (same clock as Rust)
now = get_timestamp_ns()