trialone 0.4.7__py3-none-any.whl

trialone/adapters/__init__.py

@@ -0,0 +1,9 @@
+"""Simulator adapters.
+
+Sim-agnostic interface so the core does not care which simulator runs the robot:
+instrument the world and publish the SAME /gt/* ground-truth contract regardless
+of backend. Gazebo Harmonic is the proven path (every recorded run). The isaac/
+subpackage implements the same contract for NVIDIA Isaac; its message SHAPE is
+contract-tested without a GPU (tests/test_isaac_gt_contract), but booting Isaac on
+a GPU box is not yet run (see adapters/isaac/boot_proof.py). See docs/architecture.md.
+"""

trialone/adapters/gen_ground_truth.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+"""TrialONE Sim Adapter (step 1) — generate the ground-truth ros_gz bridge for a world.
+
+The Oracle needs, from ANY world: the robot's true pose + real collisions. Hand-writing
+the ros_gz bridge for that is the painful part of onboarding a new world. This reads a
+Gazebo world SDF and emits the ground_truth_bridge.yaml automatically:
+
+  - /world/<name>/dynamic_pose/info            -> /gt/tf_dynamic     (true model poses)
+  - each <sensor type="contact"> "<x>_contact" -> /gt/contact/<x>    (real collisions)
+  - each <sensor type="camera">                -> its <topic>        (for the Recorder)
+
+Convention (docs/contracts/sim-adapter.md): name your world (not "default"), no <actor>
+elements, add contact sensors named "<x>_contact" where collisions should be caught
+(or one on your robot's base), and an overhead camera if you want video.
+
+Usage:
+  python3 src/trialone/adapters/gen_ground_truth.py worlds/walled_room.sdf
+  python3 src/trialone/adapters/gen_ground_truth.py my_world.sdf --out victim/config/ground_truth_bridge.yaml
+"""
+import argparse
+import sys
+import xml.etree.ElementTree as ET
+
+
+def _entry(ros, gz, ros_type, gz_type, comment=None):
+    head = f"# {comment}\n" if comment else ""
+    return (f"{head}- ros_topic_name: \"{ros}\"\n"
+            f"  gz_topic_name: \"{gz}\"\n"
+            f"  ros_type_name: \"{ros_type}\"\n"
+            f"  gz_type_name: \"{gz_type}\"\n"
+            f"  direction: GZ_TO_ROS\n"
+            f"  lazy: false")
+
+
+def generate(world_sdf_path):
+    root = ET.parse(world_sdf_path).getroot()
+    world = root.find("world")
+    if world is None:
+        raise ValueError("no <world> element found")
+    wname = world.get("name")
+    if not wname or wname == "default":
+        raise ValueError(f"world must be named (not 'default') for dynamic_pose/info: got {wname!r}")
+
+    blocks = [
+        "# AUTO-GENERATED by src/trialone/adapters/gen_ground_truth.py -- do not hand-edit.",
+        "# The ground truth the Oracle judges from (true pose + real collisions), bridged to ROS.",
+        "",
+        "# True pose of every non-static model (SceneBroadcaster). Robot found by proximity.",
+        _entry(f"/gt/tf_dynamic", f"/world/{wname}/dynamic_pose/info",
+               "tf2_msgs/msg/TFMessage", "gz.msgs.Pose_V"),
+    ]
+
+    contacts, cameras = [], []
+    for model in world.findall("model"):
+        mname = model.get("name")
+        for link in model.findall("link"):
+            lname = link.get("name")
+            for sensor in link.findall("sensor"):
+                stype, sname = sensor.get("type"), sensor.get("name")
+                if stype == "contact":
+                    label = sname[:-8] if sname.endswith("_contact") else sname
+                    gz = f"/world/{wname}/model/{mname}/link/{lname}/sensor/{sname}/contact"
+                    contacts.append(_entry(f"/gt/contact/{label}", gz,
+                                           "ros_gz_interfaces/msg/Contacts", "gz.msgs.Contacts"))
+                elif stype == "camera":
+                    topic = sensor.findtext("topic") or f"/{sname}"
+                    cameras.append(_entry(topic, topic,
+                                          "sensor_msgs/msg/Image", "gz.msgs.Image"))
+
+    if contacts:
+        blocks.append("\n# Real collisions (one entry per contact sensor in the world).")
+        blocks.extend(contacts)
+    if cameras:
+        blocks.append("\n# Camera(s) for the Recorder.")
+        blocks.extend(cameras)
+    return "\n".join(blocks) + "\n", len(contacts), len(cameras)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Generate the ground-truth ros_gz bridge from a world SDF.")
+    parser.add_argument("world", help="path to the Gazebo world .sdf")
+    parser.add_argument("--out", default=None, help="write here instead of stdout")
+    args = parser.parse_args()
+
+    text, n_contacts, n_cams = generate(args.world)
+    if args.out:
+        with open(args.out, "w", encoding="utf-8") as f:
+            f.write(text)
+        print(f"wrote {args.out}: dynamic_pose + {n_contacts} contact(s) + {n_cams} camera(s)",
+              file=sys.stderr)
+    else:
+        sys.stdout.write(text)
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

trialone/adapters/gen_map.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+"""TrialONE Sim Adapter (step 3) — generate a Nav2 occupancy map from a world SDF.
+
+Nav2/AMCL need a static occupancy map. Hand-drawing one per world is painful and must
+stay in sync with the SDF. This rasterizes the world's static BOX obstacles into a .pgm
++ .yaml, and can EXCLUDE models by name — e.g. the unmapped crash target, so the global
+planner doesn't know about it (the whole point of the spoof/drop demo).
+
+Assumes axis-aligned boxes (yaw ~ 0), which covers walls + box obstacles.
+
+Usage:
+  python3 src/trialone/adapters/gen_map.py worlds/corridor.sdf --out victim/maps/corridor --exclude crash_box
+  -> writes victim/maps/corridor.pgm + victim/maps/corridor.yaml
+"""
+import argparse
+import os
+import sys
+import xml.etree.ElementTree as ET
+
+RES = 0.05      # m / pixel
+MARGIN = 0.5    # m of free space padded around the obstacle bounds
+FREE, OCC = 254, 0
+
+
+def _boxes(world, exclude):
+    """Yield (cx, cy, sx, sy) for each static, axis-aligned box obstacle to map."""
+    for model in world.findall("model"):
+        name = (model.get("name") or "").lower()
+        if name in ("ground_plane", "ground") or name in exclude:
+            continue
+        for link in model.findall("link"):
+            lp = (link.findtext("pose") or "0 0 0 0 0 0").split()
+            lx, ly = float(lp[0]), float(lp[1])
+            for col in link.findall("collision"):
+                box = col.find("geometry/box/size")
+                if box is None:
+                    continue
+                sx, sy = (float(v) for v in box.text.split()[:2])
+                # collision can have its own offset pose too
+                cp = (col.findtext("pose") or "0 0 0 0 0 0").split()
+                yield (lx + float(cp[0]), ly + float(cp[1]), sx, sy)
+
+
+def generate(world_sdf_path, exclude):
+    world = ET.parse(world_sdf_path).getroot().find("world")
+    if world is None:
+        raise ValueError("no <world> element")
+    boxes = list(_boxes(world, {e.lower() for e in exclude}))
+    if not boxes:
+        raise ValueError("no static box obstacles found to map")
+
+    xmin = min(cx - sx / 2 for cx, cy, sx, sy in boxes) - MARGIN
+    xmax = max(cx + sx / 2 for cx, cy, sx, sy in boxes) + MARGIN
+    ymin = min(cy - sy / 2 for cx, cy, sx, sy in boxes) - MARGIN
+    ymax = max(cy + sy / 2 for cx, cy, sx, sy in boxes) + MARGIN
+    W = int(round((xmax - xmin) / RES))
+    H = int(round((ymax - ymin) / RES))
+
+    px = bytearray([FREE]) * (W * H)
+    for row in range(H):
+        wy = ymin + (H - 1 - row + 0.5) * RES   # image row 0 = top -> flip y
+        for col in range(W):
+            wx = xmin + (col + 0.5) * RES
+            for cx, cy, sx, sy in boxes:
+                if abs(wx - cx) <= sx / 2 and abs(wy - cy) <= sy / 2:
+                    px[row * W + col] = OCC
+                    break
+    return px, W, H, (xmin, ymin)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Generate a Nav2 occupancy map from a world SDF.")
+    parser.add_argument("world")
+    parser.add_argument("--out", required=True, help="output path stem (writes .pgm + .yaml)")
+    parser.add_argument("--exclude", nargs="*", default=[],
+                        help="model names to leave OUT of the map (e.g. the unmapped crash target)")
+    args = parser.parse_args()
+
+    px, W, H, (ox, oy) = generate(args.world, args.exclude)
+    pgm = args.out + ".pgm"
+    yaml_path = args.out + ".yaml"
+    with open(pgm, "wb") as f:
+        f.write(f"P5\n{W} {H}\n255\n".encode("ascii"))
+        f.write(bytes(px))
+    with open(yaml_path, "w", encoding="utf-8") as f:
+        f.write(
+            f"image: {os.path.basename(pgm)}\n"
+            f"mode: trinary\nresolution: {RES}\n"
+            f"origin: [{ox:.3f}, {oy:.3f}, 0.0]\n"
+            f"negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.25\n"
+        )
+    occ = sum(1 for b in px if b == OCC)
+    print(f"wrote {pgm} ({W}x{H}, {occ} occupied px) + {yaml_path} (origin {ox:.2f},{oy:.2f})",
+          file=sys.stderr)
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

trialone/adapters/instrument_world.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+"""TrialONE Sim Adapter (step 2) — instrument a bare world for TrialONE.
+
+Takes a plain Gazebo world SDF and adds everything the Oracle + Recorder need, so a
+user doesn't hand-edit SDF:
+  - the required world systems (Physics, UserCommands, SceneBroadcaster, Sensors,
+    Imu, Contact) if missing,
+  - a contact sensor on every static obstacle link (named "<link>_contact") so
+    collisions are detected,
+  - an overhead camera (if none) for the Recorder.
+
+Then run gen_ground_truth.py on the OUTPUT to get the ros_gz bridge.
+
+Usage:
+  python3 src/trialone/adapters/instrument_world.py bare.sdf --out instrumented.sdf
+"""
+import argparse
+import sys
+import xml.etree.ElementTree as ET
+
+# Required world-system plugins: (filename, name, optional (child_tag, child_text)).
+PLUGINS = [
+    ("gz-sim-physics-system", "gz::sim::systems::Physics", None),
+    ("gz-sim-user-commands-system", "gz::sim::systems::UserCommands", None),
+    ("gz-sim-scene-broadcaster-system", "gz::sim::systems::SceneBroadcaster", None),
+    ("gz-sim-sensors-system", "gz::sim::systems::Sensors", ("render_engine", "ogre2")),
+    ("gz-sim-imu-system", "gz::sim::systems::Imu", None),
+    ("gz-sim-contact-system", "gz::sim::systems::Contact", None),
+]
+
+
+def _is_ground(model):
+    """Skip the ground plane: huge, and a contact sensor on it is useless noise."""
+    if (model.get("name") or "").lower() in ("ground_plane", "ground"):
+        return True
+    return model.find(".//collision/geometry/plane") is not None
+
+
+def instrument(world_sdf_path, cam_xyz):
+    tree = ET.parse(world_sdf_path)
+    root = tree.getroot()
+    world = root.find("world")
+    if world is None:
+        raise ValueError("no <world> element found")
+    if not world.get("name") or world.get("name") == "default":
+        world.set("name", "trialone_world")  # dynamic_pose/info needs a real name
+
+    added = {"plugins": 0, "contacts": 0, "camera": 0}
+
+    # 1) World systems (add missing, by plugin name).
+    have = {p.get("name") for p in world.findall("plugin")}
+    for i, (filename, name, child) in enumerate(PLUGINS):
+        if name in have:
+            continue
+        p = ET.Element("plugin", {"filename": filename, "name": name})
+        if child:
+            ET.SubElement(p, child[0]).text = child[1]
+        world.insert(i, p)
+        added["plugins"] += 1
+
+    # 2) Contact sensor on every static obstacle link that has a named collision.
+    for model in world.findall("model"):
+        if _is_ground(model):
+            continue
+        for link in model.findall("link"):
+            if link.find("sensor[@type='contact']") is not None:
+                continue  # already instrumented
+            col = link.find("collision[@name]")
+            if col is None:
+                continue
+            lname = link.get("name") or "link"
+            sensor = ET.SubElement(link, "sensor",
+                                   {"name": f"{lname}_contact", "type": "contact"})
+            contact = ET.SubElement(sensor, "contact")
+            ET.SubElement(contact, "collision").text = col.get("name")
+            ET.SubElement(sensor, "update_rate").text = "30"
+            added["contacts"] += 1
+
+    # 3) Overhead camera if none exists anywhere.
+    if world.find(".//sensor[@type='camera']") is None:
+        cx, cy, cz = cam_xyz
+        m = ET.SubElement(world, "model", {"name": "overhead_cam"})
+        ET.SubElement(m, "static").text = "true"
+        ET.SubElement(m, "pose").text = f"{cx} {cy} {cz} 0 1.5708 0"
+        link = ET.SubElement(m, "link", {"name": "link"})
+        s = ET.SubElement(link, "sensor", {"name": "overhead_camera", "type": "camera"})
+        ET.SubElement(s, "always_on").text = "1"
+        ET.SubElement(s, "update_rate").text = "12"
+        ET.SubElement(s, "topic").text = "/overhead/image"
+        cam = ET.SubElement(s, "camera")
+        ET.SubElement(cam, "horizontal_fov").text = "1.2"
+        img = ET.SubElement(cam, "image")
+        ET.SubElement(img, "width").text = "960"
+        ET.SubElement(img, "height").text = "540"
+        ET.SubElement(img, "format").text = "R8G8B8"
+        clip = ET.SubElement(cam, "clip")
+        ET.SubElement(clip, "near").text = "0.1"
+        ET.SubElement(clip, "far").text = "50"
+        added["camera"] = 1
+
+    ET.indent(tree, space="  ")
+    return tree, added
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Instrument a bare world SDF for TrialONE.")
+    parser.add_argument("world", help="path to the bare Gazebo world .sdf")
+    parser.add_argument("--out", default=None, help="write here instead of stdout")
+    parser.add_argument("--cam-x", type=float, default=0.0)
+    parser.add_argument("--cam-y", type=float, default=0.0)
+    parser.add_argument("--cam-z", type=float, default=7.0, help="overhead camera height (m)")
+    args = parser.parse_args()
+
+    tree, added = instrument(args.world, (args.cam_x, args.cam_y, args.cam_z))
+    out = args.out or "-"
+    if args.out:
+        tree.write(args.out, encoding="unicode", xml_declaration=True)
+    else:
+        sys.stdout.write(ET.tostring(tree.getroot(), encoding="unicode"))
+    print(f"instrumented -> {out}: +{added['plugins']} systems, "
+          f"+{added['contacts']} contact sensors, +{added['camera']} camera",
+          file=sys.stderr)
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

trialone/adapters/isaac/boot_proof.py

@@ -0,0 +1,110 @@
+"""
+TrialONE Isaac adapter — BOOT PROOF smoke test.
+
+The one thing our Isaac path has never had: a committed, runnable proof that the
+stack actually boots on a real GPU box. Everything in ``sim_main.py`` is "needs the
+GPU to validate, deferred". This script closes that gap: run it on the Ubuntu + RTX
+box and it proves — in stages, so a failure localises — that the SAME boot path
+``sim_main.py`` uses is alive end-to-end.
+
+Run (Isaac's python, NOT system python — same as sim_main.py):
+    ${ISAAC_SIM}/python.sh -m trialone.adapters.isaac.boot_proof
+    # or:  ${ISAAC_SIM}/python.sh src/trialone/adapters/isaac/boot_proof.py
+
+Exit 0 + "BOOT PROOF: SUCCESS" on the last line = the box can run the Isaac backend.
+Any non-zero exit + the last "[stage] ..." line tells you exactly where it died.
+
+What it proves (each is a distinct failure the deferred path could hide):
+  1. SimulationApp boots headless                          -> GPU + Isaac install OK
+  2. isaacsim.ros2.bridge extension loads                  -> ROS 2 bringup will work
+  3. World + default ground plane builds + world.reset()   -> scene graph OK
+  4. world.step() advances physics AND world.current_time  -> the sim clock ticks
+     (our GroundTruthPublisher publishes /clock off current_time — a frozen clock is
+      a silent killer: topics look "up" but never advance)
+
+Design notes stolen (as mechanism, not code) from robocurve/inspect-robots (MIT),
+plugins/inspect-robots-isaacsim/scripts/boot_proof.py — two hard-won Isaac lessons:
+  * Print the verdict + flush BEFORE app.close(): Isaac's close() does a hard
+    os._exit that swallows buffered stdout, so a verdict printed after close()
+    silently vanishes and the run looks like it hung.
+  * Time the boot: a "success" that took 400 s tells you the box is thermal/paging
+    limited even when it technically worked.
+"""
+from __future__ import annotations
+
+import sys
+import time
+
+
+def _stage(msg: str) -> None:
+    # flush every stage line so the last line before a crash is always visible
+    print(f"[stage] {msg}", flush=True)
+
+
+def main() -> int:
+    t0 = time.perf_counter()
+
+    # 1. SimulationApp — MUST be created before any other isaac/omni import
+    #    (identical dict to sim_main.py so we prove the exact same boot).
+    _stage("launching SimulationApp (headless, RayTracedLighting)...")
+    from isaacsim import SimulationApp
+
+    app = SimulationApp({"headless": True, "renderer": "RayTracedLighting"})
+    t_app = time.perf_counter() - t0
+    running = getattr(app, "is_running", lambda: None)()
+    _stage(f"SimulationApp live in {t_app:.1f}s  is_running={running}")
+
+    # 2. ROS 2 bridge extension — the bringup script depends on this loading
+    _stage("enabling isaacsim.ros2.bridge extension...")
+    from isaacsim.core.utils.extensions import enable_extension
+
+    enable_extension("isaacsim.ros2.bridge")
+    app.update()
+    _stage("ros2.bridge enabled")
+
+    # 3. World + ground plane + reset (the scene graph sim_main.py builds)
+    _stage("building World + default ground plane...")
+    from isaacsim.core.api import World
+
+    world = World(stage_units_in_meters=1.0)
+    world.scene.add_default_ground_plane()
+    world.reset()
+    _stage("World reset OK")
+
+    # 4. Step physics and prove the sim CLOCK actually advances (not just that
+    #    step() returns). current_time frozen -> /clock frozen -> every downstream
+    #    topic looks alive but time never moves. This is the check that matters.
+    _stage("stepping physics x60, watching world.current_time...")
+    t_before = float(world.current_time)
+    for _ in range(60):
+        world.step(render=False)
+    t_after = float(world.current_time)
+    dt = float(world.get_physics_dt())
+    advanced = t_after - t_before
+    _stage(f"current_time {t_before:.3f} -> {t_after:.3f} (+{advanced:.3f}s, physics_dt={dt:.4f})")
+
+    clock_ok = advanced > 0.0
+    total = time.perf_counter() - t0
+
+    # Verdict BEFORE close(): app.close() hard-exits and eats buffered stdout.
+    if clock_ok:
+        print(
+            f"BOOT PROOF: SUCCESS  (boot {t_app:.1f}s, full stack {total:.1f}s, "
+            f"sim clock advanced +{advanced:.3f}s over 60 steps)",
+            flush=True,
+        )
+        rc = 0
+    else:
+        print(
+            "BOOT PROOF: FAILURE  (SimulationApp booted but the sim clock did NOT "
+            "advance — /clock would be frozen; check physics_dt / world.play state)",
+            flush=True,
+        )
+        rc = 1
+
+    app.close()  # may hard-exit here; the verdict above is already flushed
+    return rc
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

trialone/adapters/isaac/gen_ground_truth_isaac.py

@@ -0,0 +1,59 @@
+"""
+TrialONE Isaac adapter — ground-truth config from a world manifest.
+
+The USD analog of ``gen_ground_truth.py``. Instead of parsing a world SDF for contact
+sensors + the dynamic-pose source, read a tiny hand-written sidecar manifest (JSON next
+to the ``.usd``) that names: the robot's dynamic prim(s), the contact-link prim(s) +
+labels, and the overhead camera. Generic USD introspection (auto-inject the hooks) is a
+later increment — exactly mirroring the ``[planned]`` auto-inject status on the Gazebo
+side, so we don't repeat that scope creep here.
+
+Manifest shape (see ``worlds/walled_room.isaac.json``)::
+
+    {
+      "world":        "worlds/walled_room.usd",          # optional; ground plane if absent
+      "robot_usd":    ".../nova_carter.usd",
+      "robot_prim":   "/World/robot",
+      "dynamic_prims": { "robot": "/World/robot" },       # model_name -> prim path
+      "contacts":     [ { "label": "base", "prim": "/World/robot/chassis_link" } ],
+      "camera":       { "prim": "/World/overhead_cam", "topic": "/overhead/image",
+                        "resolution": [640, 480], "position": [0, 0, 8] },
+      "cmd_vel":      { "topic": "/cmd_vel", "wheel_radius": 0.14, "wheel_base": 0.41 }
+    }
+"""
+from __future__ import annotations
+
+import json
+from typing import Any, Dict
+
+
+def load_manifest(path: str) -> Dict[str, Any]:
+    """Read a world manifest JSON."""
+    with open(path) as f:
+        return json.load(f)
+
+
+def to_gt_config(manifest: Dict[str, Any]) -> Dict[str, Any]:
+    """Normalise a manifest into the config ``sim_main`` / ``GroundTruthPublisher`` consume."""
+    cfg = {
+        "world": manifest.get("world"),
+        "robot_usd": manifest.get("robot_usd"),
+        "robot_prim": manifest.get("robot_prim", "/World/robot"),
+        "dynamic_prims": dict(manifest.get("dynamic_prims", {})),
+        "contacts": list(manifest.get("contacts", [])),
+        "camera": manifest.get("camera"),
+        "cmd_vel": manifest.get("cmd_vel"),
+    }
+    # A robot with no explicit dynamic_prims still needs its base tracked for /gt/tf_dynamic.
+    if not cfg["dynamic_prims"]:
+        cfg["dynamic_prims"] = {"robot": cfg["robot_prim"]}
+    # Validate the pieces the contract depends on.
+    for c in cfg["contacts"]:
+        if "label" not in c or "prim" not in c:
+            raise ValueError(f"contact spec needs 'label' and 'prim': {c}")
+    return cfg
+
+
+if __name__ == "__main__":  # quick CLI: print the normalised config for a manifest
+    import sys
+    print(json.dumps(to_gt_config(load_manifest(sys.argv[1])), indent=2))

trialone/adapters/isaac/gt_publisher.py

@@ -0,0 +1,137 @@
+"""
+TrialONE Isaac adapter — ground-truth publisher (the contract-critical piece).
+
+Publishes the SAME ground-truth ROS 2 topics the Gazebo ``ros_gz`` bridge does, so the
+Oracle, injectors and recorder work UNCHANGED across backends (see
+``docs/contracts/sim-adapter.md``). Only the backend differs:
+
+  /gt/tf_dynamic        tf2_msgs/TFMessage           true world pose of every dynamic model
+  /gt/contact/<label>   ros_gz_interfaces/Contacts   non-empty ``.contacts`` == a real collision
+  /clock                rosgraph_msgs/Clock          sim time (so the rest of the stack syncs)
+
+State is read from *pluggable sources* so this node can be driven by Isaac (reading the
+USD stage + a ContactSensor) in production AND by a fake source in a CPU-only contract
+test (no Isaac, no GPU) — the test asserts the real Oracle accepts these exact messages.
+That keeps the riskiest unknown (message shape) verifiable off the GPU box.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Callable, Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+from rosgraph_msgs.msg import Clock
+from ros_gz_interfaces.msg import Contacts, Contact
+
+# pose_source() -> {model_name: (x, y, z[, qx, qy, qz, qw])} in the world frame, metres.
+Pose = Tuple[float, ...]
+PoseSource = Callable[[], Dict[str, Pose]]
+# contact_source() -> True iff that link is currently in contact with anything.
+ContactSource = Callable[[], bool]
+# clock_source() -> sim time in seconds (float), or None to use the node's wall clock.
+ClockSource = Optional[Callable[[], float]]
+
+
+@dataclass
+class ContactSpec:
+    """One ``/gt/contact/<label>`` sensor: a label and a predicate that is True on contact."""
+    label: str
+    source: ContactSource
+
+
+class GroundTruthPublisher(Node):
+    """Republishes Isaac true-state on the Gazebo-compatible ``/gt/*`` topics.
+
+    Drive it by calling :meth:`tick` once per simulation step (after ``world.step``).
+    """
+
+    def __init__(
+        self,
+        pose_source: PoseSource,
+        contact_specs: List[ContactSpec],
+        clock_source: ClockSource = None,
+        world_frame: str = "world",
+        node_name: str = "trialone_gt_publisher",
+    ) -> None:
+        super().__init__(node_name)
+        self._pose_source = pose_source
+        self._contacts = contact_specs
+        self._clock_source = clock_source
+        self._world_frame = world_frame
+
+        self._tf_pub = self.create_publisher(TFMessage, "/gt/tf_dynamic", 10)
+        self._clock_pub = self.create_publisher(Clock, "/clock", 10)
+        self._contact_pubs = {
+            cs.label: self.create_publisher(Contacts, f"/gt/contact/{cs.label}", 10)
+            for cs in contact_specs
+        }
+        self.get_logger().info(
+            "GT publisher up: /gt/tf_dynamic, /clock, "
+            + str([f"/gt/contact/{cs.label}" for cs in contact_specs])
+        )
+
+    # --- time -------------------------------------------------------------
+    def _stamp(self):
+        """ROS time stamp; from the sim clock if available so all stamps share sim time."""
+        if self._clock_source is not None:
+            sec = float(self._clock_source())
+            stamp = rclpy.time.Time(seconds=sec).to_msg()
+            return stamp
+        return self.get_clock().now().to_msg()
+
+    def publish_clock(self) -> None:
+        if self._clock_source is None:
+            return
+        msg = Clock()
+        msg.clock = self._stamp()
+        self._clock_pub.publish(msg)
+
+    # --- ground truth -----------------------------------------------------
+    def publish_tf_dynamic(self) -> None:
+        """One ``TFMessage`` holding the world pose of every dynamic model.
+
+        Matches the Gazebo ``Pose_V`` -> ``TFMessage`` bridge shape: the Oracle
+        proximity-picks the robot from ``msg.transforms`` by translation (dropping
+        entries with |x|,|y| > 6 or |z| > 2), so the only hard requirement is that the
+        robot's transform carries its true world (x, y, z) in metres.
+        """
+        poses = self._pose_source()
+        msg = TFMessage()
+        stamp = self._stamp()
+        for name, p in poses.items():
+            ts = TransformStamped()
+            ts.header.stamp = stamp
+            ts.header.frame_id = self._world_frame
+            ts.child_frame_id = name
+            ts.transform.translation.x = float(p[0])
+            ts.transform.translation.y = float(p[1])
+            ts.transform.translation.z = float(p[2])
+            if len(p) >= 7:
+                ts.transform.rotation.x = float(p[3])
+                ts.transform.rotation.y = float(p[4])
+                ts.transform.rotation.z = float(p[5])
+                ts.transform.rotation.w = float(p[6])
+            else:
+                ts.transform.rotation.w = 1.0
+            msg.transforms.append(ts)
+        self._tf_pub.publish(msg)
+
+    def publish_contacts(self) -> None:
+        """Per sensor: publish ``Contacts`` whose ``.contacts`` is non-empty iff in contact."""
+        stamp = self._stamp()
+        for cs in self._contacts:
+            msg = Contacts()
+            msg.header.stamp = stamp
+            msg.header.frame_id = self._world_frame
+            if cs.source():
+                msg.contacts.append(Contact())  # one entry -> Oracle reads len>0 == collision
+            self._contact_pubs[cs.label].publish(msg)
+
+    def tick(self) -> None:
+        """Publish all ground-truth topics for the current step (call after ``world.step``)."""
+        self.publish_clock()
+        self.publish_tf_dynamic()
+        self.publish_contacts()