From 70eedb43dff40687b0c9ca93cbad3f26c2a90fd5 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Thu, 4 Jun 2026 15:01:44 -0400
Subject: [PATCH 01/27] WIP implementation

---
 .../emulator/server/natnet_data_types.py      | 245 ++++++++++++++++
 .../natnet/emulator/server/natnet_server.py   | 270 ++++++++++++++++++
 .../emulator/server/natnet_server_types.py    | 118 ++++++++
 .../emulator/server/natnet_unicast_server.py  | 104 +++++++
 4 files changed, 737 insertions(+)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
new file mode 100644
index 000000000..f9d41ec5d
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
@@ -0,0 +1,245 @@
+import ctypes
+import struct
+from enum import Enum, IntEnum
+
+class ModelLimits(IntEnum):
+    MAX_MODELS = 2000    # maximum number of total models (data descriptions)
+    MAX_MARKERSETS = 1000    # maximum number of MarkerSets 
+    MAX_RIGIDBODIES = 1000    # maximum number of RigidBodies
+    MAX_ASSETS = 1000    # Maximum number of Assets
+    MAX_NAMELENGTH = 256     # maximum length for strings
+    MAX_MARKERS = 200     # maximum number of markers per MarkerSet
+    MAX_RBMARKERS = 20      # maximum number of markers per RigidBody
+    MAX_SKELETONS = 100     # maximum number of skeletons
+    MAX_SKELRIGIDBODIES = 200     # maximum number of RididBodies per Skeleton
+    MAX_LABELED_MARKERS = 1000    # maximum number of labeled markers per frame
+    MAX_UNLABELED_MARKERS = 1000    # maximum number of unlabeled (other) markers per frame
+
+    MAX_FORCEPLATES = 100     # maximum number of force plate 'bundles'
+    MAX_DEVICES = 100     # maximum number of peripheral device 'bundles'
+    MAX_ANALOG_CHANNELS = 32      # maximum number of data channels (signals) per analog/force plate device
+    MAX_ANALOG_SUBFRAMES = 30      # maximum number of analog/force plate frames per mocap frame
+
+    MAX_PACKETSIZE = 65503   # max size of packet in bytes (actual packet size is dynamic)
+                                                # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes)
+
+class sMarker(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("ID", ctypes.c_int32),
+        ("x", ctypes.c_float),
+        ("y", ctypes.c_float),
+        ("z", ctypes.c_float),
+        ("size", ctypes.c_float),
+        ("params", ctypes.c_int16),
+        ("residual", ctypes.c_float)
+    ]
+
+    def pack(self) -> bytes:
+        return struct.pack('<i5fhf', self.ID, self.x, self.y, self.z, self.size, self.params, self.residual)
+
+MarkerData = ctypes.c_float * 3
+
+class sMarkerSetData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("szName", ctypes.c_char * ModelLimits.MAX_NAMELENGTH),            # MarkerSet name
+        ("nMarkers", ctypes.c_int32),                       # # of markers in MarkerSet
+        ("Markers", MarkerData * ModelLimits.MAX_MARKERS)   # Using max array, pack will chunk it
+    ]
+
+    def pack(self) -> bytes:
+        # szName is null-terminated rather than fixed 256 over network
+        name_bytes = self.szName.rstrip(b'\x00') + b'\x00'
+        payload = bytearray(name_bytes)
+        payload += struct.pack('<i', self.nMarkers)
+        for i in range(self.nMarkers):
+            payload += struct.pack('<3f', self.Markers[i][0], self.Markers[i][1], self.Markers[i][2])
+        return bytes(payload)
+
+class sRigidBodyData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("ID", ctypes.c_int32),
+        ("x", ctypes.c_float),
+        ("y", ctypes.c_float),
+        ("z", ctypes.c_float),
+        ("qx", ctypes.c_float),
+        ("qy", ctypes.c_float),
+        ("qz", ctypes.c_float),
+        ("qw", ctypes.c_float),
+        ("MeanError", ctypes.c_float),
+        ("params", ctypes.c_int16),
+    ]
+
+    def pack(self) -> bytes:
+        return struct.pack('<i8fh', 
+            self.ID, 
+            self.x, self.y, self.z, 
+            self.qx, self.qy, self.qz, self.qw, 
+            self.MeanError, self.params
+        )
+
+class sSkeletonData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("skeletonID", ctypes.c_int32),
+        ("nRigidBodies", ctypes.c_int32),
+        ("RigidBodies", sRigidBodyData * ModelLimits.MAX_SKELRIGIDBODIES),
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack('<ii', self.skeletonID, self.nRigidBodies))
+        for i in range(self.nRigidBodies):
+            payload += self.RigidBodies[i].pack()
+        return bytes(payload)
+
+class sAssetData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("assetID", ctypes.c_int32),
+        ("nRigidBodies", ctypes.c_int32),
+        ("RigidBodies", sRigidBodyData * ModelLimits.MAX_SKELRIGIDBODIES),
+        ("nMarkers", ctypes.c_int32),
+        ("Markers", sMarker * ModelLimits.MAX_MARKERS)
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack('<ii', self.assetID, self.nRigidBodies))
+        for i in range(self.nRigidBodies):
+            payload += self.RigidBodies[i].pack()
+        payload += struct.pack('<i', self.nMarkers)
+        for i in range(self.nMarkers):
+            payload += self.Markers[i].pack()
+        return bytes(payload)
+
+class sAnalogChannelData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+         ("nFrames", ctypes.c_int32),
+         ("Values", ctypes.c_float * ModelLimits.MAX_ANALOG_SUBFRAMES)
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack('<i', self.nFrames))
+        for i in range(self.nFrames):
+            payload += struct.pack('<f', self.Values[i])
+        return bytes(payload)
+
+class sForcePlateData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("ID", ctypes.c_int32),
+        ("nChannels", ctypes.c_int32),
+        ("ChannelData", sAnalogChannelData * ModelLimits.MAX_ANALOG_CHANNELS),
+        ("params", ctypes.c_int16),
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack('<ii', self.ID, self.nChannels))
+        for i in range(self.nChannels):
+            payload += self.ChannelData[i].pack()
+        payload += struct.pack('<h', self.params)
+        return bytes(payload)
+
+class sDeviceData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("ID", ctypes.c_int32),
+        ("nChannels", ctypes.c_int32),
+        ("ChannelData", sAnalogChannelData * ModelLimits.MAX_ANALOG_CHANNELS),
+        ("params", ctypes.c_int16),
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack('<ii', self.ID, self.nChannels))
+        for i in range(self.nChannels):
+            payload += self.ChannelData[i].pack()
+        payload += struct.pack('<h', self.params)
+        return bytes(payload)
+
+class sFrameOfMocapData(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("iFrame", ctypes.c_int32),
+
+        ("nMarkerSets", ctypes.c_int32),
+        ("MocapData", sMarkerSetData * ModelLimits.MAX_MARKERSETS),
+
+        ("nOtherMarkers", ctypes.c_int32),
+        ("OtherMarkers", MarkerData * ModelLimits.MAX_UNLABELED_MARKERS),
+
+        ("nRigidBodies", ctypes.c_int32),
+        ("RigidBodies", sRigidBodyData * ModelLimits.MAX_RIGIDBODIES),
+
+        ("nSkeletons", ctypes.c_int32),
+        ("Skeletons", sSkeletonData * ModelLimits.MAX_SKELETONS),
+
+        ("nAssets", ctypes.c_int32),
+        ("Assets", sAssetData * ModelLimits.MAX_ASSETS),
+
+        ("nLabeledMarkers", ctypes.c_int32),
+        ("LabeledMarkers", sMarker * ModelLimits.MAX_LABELED_MARKERS),
+
+        ("nForcePlates", ctypes.c_int32),
+        ("ForcePlates", sForcePlateData * ModelLimits.MAX_FORCEPLATES),
+
+        ("nDevices", ctypes.c_int32),
+        ("Devices", sDeviceData * ModelLimits.MAX_DEVICES),
+
+        ("Timecode", ctypes.c_uint32),
+        ("TimecodeSubframe", ctypes.c_uint32),
+        ("fTimestamp", ctypes.c_double),
+        ("CameraMidExposureTimestamp", ctypes.c_uint64),
+        ("CameraDataReceivedTimestamp", ctypes.c_uint64),
+        ("TransmitTimestamp", ctypes.c_uint64),    
+        ("PrecisionTimestampSecs", ctypes.c_uint32),
+        ("PrecisionTimestampFractionalSecs", ctypes.c_uint32),
+        ("params", ctypes.c_int16)
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray()
+        
+        payload += struct.pack('<i', self.iFrame)
+
+        payload += struct.pack('<i', self.nMarkerSets)
+        for i in range(self.nMarkerSets):
+            payload += self.MocapData[i].pack()
+
+        payload += struct.pack('<i', self.nOtherMarkers)
+        for i in range(self.nOtherMarkers):
+            payload += struct.pack('<3f', self.OtherMarkers[i][0], self.OtherMarkers[i][1], self.OtherMarkers[i][2])
+
+        payload += struct.pack('<i', self.nRigidBodies)
+        for i in range(self.nRigidBodies):
+            payload += self.RigidBodies[i].pack()
+
+        payload += struct.pack('<i', self.nSkeletons)
+        for i in range(self.nSkeletons):
+            payload += self.Skeletons[i].pack()
+
+        payload += struct.pack('<i', self.nAssets)
+        for i in range(self.nAssets):
+            payload += self.Assets[i].pack()
+
+        payload += struct.pack('<i', self.nLabeledMarkers)
+        for i in range(self.nLabeledMarkers):
+            payload += self.LabeledMarkers[i].pack()
+
+        payload += struct.pack('<i', self.nForcePlates)
+        for i in range(self.nForcePlates):
+            payload += self.ForcePlates[i].pack()
+
+        payload += struct.pack('<i', self.nDevices)
+        for i in range(self.nDevices):
+            payload += self.Devices[i].pack()
+
+        # NatNet 3.0+ Timecodes & stamps
+        payload += struct.pack('<II', self.Timecode, self.TimecodeSubframe)
+        payload += struct.pack('<d', self.fTimestamp)
+        payload += struct.pack('<QQQ', self.CameraMidExposureTimestamp, self.CameraDataReceivedTimestamp, self.TransmitTimestamp)
+        payload += struct.pack('<II', self.PrecisionTimestampSecs, self.PrecisionTimestampFractionalSecs)
+        payload += struct.pack('<h', self.params)
+
+        return bytes(payload)   
\ No newline at end of file
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
new file mode 100644
index 000000000..e5e4ef7bd
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
@@ -0,0 +1,270 @@
+from typing import Any
+
+
+from . import natnet_data_types as DataMessages
+from . import natnet_server_types as ServerMessages
+from enum import Enum
+import socket
+import threading
+import queue
+import time
+import signal
+import ctypes
+import typing
+
+
+class TransmissionType(str, Enum):
+    UNICAST = "unicast"
+    MULTICAST = "multicast"
+
+class Client:
+    def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)):
+        self.ip = ip
+        self.port = port
+        self.version = version
+        self.subscribed_assets = set()
+        self.socket_lock = threading.Lock()
+
+    def __hash__(self):
+        # We uniquely identify a client session by their IP and their unique command port.
+        # Why? A single machine could technically run multiple separate NatNet clients
+        # simultaneously, and they would share an IP but have unique command ports.
+        return hash((self.ip, self.port))
+
+    def __eq__(self, other):
+        return (isinstance(other, Client) and 
+                self.ip == other.ip and 
+                self.port == other.port)
+    
+
+class NatNetServer:
+    def __init__(self, 
+            local_interface : str = "172.31.0.200",
+            transmission_type: TransmissionType = TransmissionType.MULTICAST, 
+            multicast_address : str = "239.255.42.99",
+            command_port: int = 1510,
+            data_port : int = 1511,
+            motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0),
+            natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0),
+            high_res_clock_freq : int = 1_000_000_000, 
+            publish_rate : int = 100 # Hz (default 100Hz)
+        ):
+
+        self.local_interface = local_interface
+        self.transmission_type = transmission_type
+        self.multicast_address = multicast_address
+        self.command_port = command_port
+        self.data_port = data_port
+        self.motive_app_version = motive_app_version
+        self.natnet_version = natnet_version
+        self.high_res_clock_freq = high_res_clock_freq
+        self.publish_rate = publish_rate
+        
+        self._validate_init_params()
+
+        self.server_description = self._build_server_description()
+        # Initialize synchronously safe data structures for server state and mocap data
+
+        # Thread-safe queue for Mocp frames
+        self.mocap_data_queue = queue.Queue(maxsize=100)
+        
+        # Thread list and shutdown event
+        self.threads = []
+        self.shutdown_event = threading.Event()
+
+        # Connected clients for unicast mode
+        self.connected_clients : typing.Set[Client] = set()
+        self.clients_lock : threading.Lock = threading.Lock()
+
+        # Sockets
+        self.command_socket : socket.socket | None = None
+        self.data_socket : socket.socket | None = None
+
+        self.running = False
+
+        # Start up process with threads
+        # # One thread to listen and manage command requests (TCP/UDP)
+        # # One thread to send data packets (UDP)
+        # # One thread to manage server state and data updates (mocap system or other source)
+
+        # Overall design:
+        # - On initialization, validate parameters and set up server description
+        # - Start threads for command listening and data sending
+        # - Perhaps separate into Unicast and Multicast server classes that inherit from a common base, or handle both in one class with conditional logic based on transmission type
+        # - Multicast server will need to join multicast group and manage socket options accordingly, while unicast server will send directly to client IPs
+        # - General helper functions will break packets down into appropriate sizes, serialize data messages, and manage client connections for unicast case
+        # - General main loop will manage server lifecycle and clean shutdown, while threads will handle their respective tasks for command listening and data sending
+        # - General main data management function will take in new mocap data (e.g. from mocap system or other source), update the latest data state, and trigger packet sending to clients at regular intervals (e.g. 100Hz)
+        # - Unicast:
+        # - On command request, respond with server description and data packets sent directly to requesting client's IP
+        # - Have a synchronously safe list of connected clients to manage multiple unicast recipients
+        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
+        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to unicast clients.
+        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets 
+        # - Multicast:
+        # - On command request, respond with server description but data packets will be sent to the multicast group address rather than directly to client IPs
+        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
+        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to the multicast group address.
+        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets and send to the multicast group address.
+
+    def _signal_handler(self, signum, frame):
+        print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...")
+        self.shutdown()
+
+    def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData):
+        # Thread-safe method to push new physics frames (called by Isaac-Sim extension)
+        if self.mocap_data_queue.full():
+            try:
+                # Drop oldest frame if falling behind
+                self.mocap_data_queue.get_nowait()
+            except queue.Empty:
+                pass
+        self.mocap_data_queue.put(new_data)
+
+    def start(self):
+        # Bind sockets and launch worker threads automatically on init
+        
+        # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill)
+        try:
+            signal.signal(signal.SIGINT, self._signal_handler)
+            signal.signal(signal.SIGTERM, self._signal_handler)
+        except ValueError:
+            pass # Safe fallback if not called from the main thread
+        
+        # 1. Setup Command Socket (Receives connection/discovery requests)
+        self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+        self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self.command_socket.bind(('', self.command_port))
+
+        # 2. Setup Data Socket (Sends outward Mocap frames)
+        self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+        # Note: Data socket doesn't need to bind to the multicast explicitly if it's only sending.
+        # It just routes via the local interface.
+        self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface))
+
+        # 3. Launch Threads
+        cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True)
+        data_thread = threading.Thread(target=self._data_update_loop, daemon=True)
+
+        self.threads.extend([cmd_thread, data_thread])
+        
+        for t in self.threads:
+            t.start()
+        
+        self.running = True
+
+    def shutdown(self):
+        # Cleanly shutdown threads and close sockets
+        self.running = False
+        self.shutdown_event.set()
+        
+        if self.command_socket:
+            self.command_socket.close()
+            
+        if self.data_socket:
+            self.data_socket.close()
+
+        for t in self.threads:
+            if t.is_alive():
+                t.join(timeout=1.0)
+
+    def _validate_init_params(self):
+
+        # Validate the local_interface is a valid IP address
+        if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3:
+            raise ValueError(f"Invalid local interface IP address: {self.local_interface}")
+
+        # Validate between transmission types and address requirements
+        if self.transmission_type not in TransmissionType:
+            raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.")
+
+        if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address:
+            raise ValueError("Multicast address must be provided for multicast transmission type.")
+        
+        if self.transmission_type == TransmissionType.UNICAST and self.multicast_address:
+            raise ValueError("Multicast address should not be provided for unicast transmission type.")
+
+        if not (0 < self.command_port < 65536):
+            raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.")
+
+        if not (0 < self.data_port < 65536):
+            raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.")
+        
+        if self.command_port == self.data_port:
+            raise ValueError("Command port and data port must be different.")
+
+        if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4):
+            raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
+
+        if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4):
+            raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
+        
+        if self.motive_app_version and not self.motive_app_version[0] == 3:
+            raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0")
+
+        if not self.natnet_version[0] == 4:
+            raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0")
+
+        if self.high_res_clock_freq <= 0:
+            raise ValueError(f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz.")
+
+    def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData:
+        # Thread-safe method to retrieve the latest mocap data to be sent
+        try:
+            return self.mocap_data_queue.get_nowait()
+        except queue.Empty:
+            return None
+
+    def _build_server_description(self) -> ServerMessages.sServerDescription:
+        # Helper to build the server description struct with current server info (e.g. on startup or in response to command request)
+        description = ServerMessages.sServerDescription()
+        description.HostPresent = True
+        description.szHostComputerName = socket.gethostname().encode('utf-8')[:ServerMessages.MAX_NAMELENGTH-1] + b'\x00'
+        description.HostComputerAddress = socket.inet_aton(self.local_interface)
+        description.szHostApp = b'Motive'
+        description.HostAppVersion = self.motive_app_version
+        description.NatNetVersion = self.natnet_version
+        description.HighResClockFrequency = self.high_res_clock_freq
+        description.bConnectionInfoValid = True
+        description.ConnectionDataPort = self.data_port
+        description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST
+        
+        if self.transmission_type == TransmissionType.MULTICAST:
+            description.ConnectionMulticastAddress = socket.inet_aton(self.multicast_address)
+        else:
+            description.ConnectionMulticastAddress = b'\x00\x00\x00\x00' # Setting to 0 explicitly for unicast case
+        
+        return description
+
+    def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Loop to update mocap data and send packets at regular intervals (e.g. 100Hz)
+        pass
+
+    def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData):
+        # Serialize data_messages and send as UDP packet to client or multicast group
+        
+        if not self.data_socket:
+            raise ValueError("[NatNetServer] Data socket not initialized. Cannot send data packet.")
+
+        # Serialize the data message into bytes
+        try:
+            packet_bytes = data_message.pack()
+            header = ServerMessages.sPacketHeader(iMessage=ServerMessages.MessageId.NAT_FRAMEOFDATA, nDataBytes=len(packet_bytes))
+            full_packet = header.pack() + packet_bytes
+        except Exception as e:
+            raise ValueError(f"[NatNetServer] Error serializing data message: {e}")
+        
+        try:
+            with client.socket_lock:
+                self.data_socket.sendto(full_packet, (client.ip, client.port))
+        except Exception as e:
+            raise ValueError(f"[NatNetServer] Error sending data packet to client {client.ip}:{client.port}: {e}")
+
+    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Loop to listen for and handle incoming command requests (e.g. from client apps)
+        pass
+
+    def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Parse incoming command request, perform requested action, and send response if needed
+        pass
+
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
new file mode 100644
index 000000000..ad9945a5e
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
@@ -0,0 +1,118 @@
+import ctypes
+from enum import IntEnum
+
+MAX_NAMELENGTH = 256
+MAX_PACKETSIZE = 65503
+
+# Client/server message ids
+class MessageId(IntEnum):
+    NAT_CONNECT                 = 0
+    NAT_SERVERINFO              = 1
+    NAT_REQUEST                 = 2
+    NAT_RESPONSE                = 3
+    NAT_REQUEST_MODELDEF        = 4
+    NAT_MODELDEF                = 5
+    NAT_REQUEST_FRAMEOFDATA     = 6
+    NAT_FRAMEOFDATA             = 7
+    NAT_MESSAGESTRING           = 8
+    NAT_DISCONNECT              = 9
+    NAT_KEEPALIVE               = 10
+    NAT_DISCONNECTBYTIMEOUT     = 11
+    NAT_ECHOREQUEST             = 12
+    NAT_ECHORESPONSE            = 13
+    NAT_DISCOVERY               = 14
+    NAT_UNRECOGNIZED_REQUEST    = 100
+
+# NatNet data types / Descriptors
+class DataDescriptors(IntEnum):
+    Descriptor_MarkerSet = 0
+    Descriptor_RigidBody = 1
+    Descriptor_Skeleton = 2
+    Descriptor_ForcePlate = 3
+    Descriptor_Device = 4
+    Descriptor_Camera = 5
+    Descriptor_Asset = 6
+
+# Server/Sender configuration and info
+class sSender(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name
+        ("Version", ctypes.c_uint8 * 4),            # host app's version [major.minor.build.revision]
+        ("NatNetVersion", ctypes.c_uint8 * 4)       # host app's NatNet version
+    ]
+
+    def pack(self) -> bytes:
+        return bytes(self)
+
+class sSender_Server(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("Common", sSender),
+        ("HighResClockFrequency", ctypes.c_uint64),
+        ("DataPort", ctypes.c_uint16),
+        ("IsMulticast", ctypes.c_bool),
+        ("MulticastGroupAddress", ctypes.c_uint8 * 4)
+    ]
+
+    def pack(self) -> bytes:
+        return bytes(self)
+
+# Mocap server application description
+class sServerDescription(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("HostPresent", ctypes.c_bool),
+        ("szHostComputerName", ctypes.c_char * MAX_NAMELENGTH),
+        ("HostComputerAddress", ctypes.c_uint8 * 4),
+        ("szHostApp", ctypes.c_char * MAX_NAMELENGTH),
+        ("HostAppVersion", ctypes.c_uint8 * 4),
+        ("NatNetVersion", ctypes.c_uint8 * 4),
+        ("HighResClockFrequency", ctypes.c_uint64),
+        ("bConnectionInfoValid", ctypes.c_bool),
+        ("ConnectionDataPort", ctypes.c_uint16),
+        ("ConnectionMulticast", ctypes.c_bool),
+        ("ConnectionMulticastAddress", ctypes.c_uint8 * 4)
+    ]
+
+    def pack(self) -> bytes:
+        return bytes(self)
+
+# Base packet layout
+class sPacketHeader(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("iMessage", ctypes.c_uint16),    # e.g., MessageId.NAT_FRAMEOFDATA
+        ("nDataBytes", ctypes.c_uint16),  # Bytes purely in the payload following this header
+    ]
+
+    def pack(self) -> bytes:
+        return bytes(self)
+
+# Connection types enum matching NatNet SDK rules
+class ConnectionType(IntEnum):
+    ConnectionType_Multicast = 0
+    ConnectionType_Unicast   = 1
+
+class sNatNetClientConnectParams(ctypes.Structure):
+    """
+    Python ctypes translation of the C++ sNatNetClientConnectParams struct.
+    Enforces a packed structure byte alignment matching the NatNet binary network protocol.
+    """
+    _pack_ = 1
+    _fields_ = [
+        ("connectionType", ctypes.c_int32),         # 4 bytes (mapping to standard ConnectionType enum)
+        ("serverCommandPort", ctypes.c_uint16),     # 2 bytes
+        ("serverDataPort", ctypes.c_uint16),        # 2 bytes
+        
+        # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead.
+        ("serverAddress", ctypes.c_void_p),         
+        ("localAddress", ctypes.c_void_p),          
+        ("multicastAddress", ctypes.c_void_p),      
+        
+        ("subscribedDataOnly", ctypes.c_bool),      # 1 byte
+        ("BitstreamVersion", ctypes.c_uint8 * 4)    # 4 bytes: [Major, Minor, Build, Revision]
+    ]
+
+    def pack(self) -> bytes:
+        return bytes(self)
\ No newline at end of file
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
new file mode 100644
index 000000000..a4186a24c
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -0,0 +1,104 @@
+from . import natnet_data_types as DataTypes
+from . import natnet_server_types as ServerTypes
+from enum import Enum
+import socket
+import threading
+import queue
+import time
+import signal
+import ctypes
+from .natnet_server import TransmissionType, Client, NatNetServer
+
+
+class NatNetUnicastServer(NatNetServer):
+    def __init__(self, 
+            local_interface="172.31.0.200",
+            transmission_type: TransmissionType = TransmissionType.UNICAST, 
+            multicast_address=None,
+            command_port=1510,
+            data_port=1511
+        ):
+
+        if not transmission_type == TransmissionType.UNICAST:
+            raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.")
+
+        super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port)
+
+    def _data_update_loop(self):
+        # Loop to update mocap data and send packets at regular intervals
+        while not self.shutdown_event.is_set():
+            time.sleep(1/self.publish_rate) # Sleep for the duration of one publish cycle
+
+            data_messages = self._get_latest_mocap_packet()
+            if not data_messages:
+                continue
+
+            with self.clients_lock:
+                for client in self.connected_clients:
+                    try:
+                        self._send_data_packet(client, data_messages) # Thread-safely sends data packet to all connected clients
+                    except ValueError as e:
+                        print(str(e))
+                        continue
+
+    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Listens on UDP command socket for incoming command requests from clients.
+        # Handles incoming client handshakes and teardown.
+
+        print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...")
+
+        while not self.shutdown_event.is_set():
+            try:
+                data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests
+                if not data:
+                    continue
+                print(f"[Command Listener] Received command request from client {addr}.")
+                self._handle_command_request(data, addr)
+            except Exception as e:
+                if self.shutdown_event.is_set():
+                    break
+                print(f"[Command Listener] Error receiving command request: {e}")
+                time.sleep(0.1) # Sleep briefly to avoid tight loop on errors
+    
+    def _handle_command_request(self, request_data: bytes, client_address: tuple): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        """
+        Processes standard binary headers and registers unicast endpoints.
+        """
+        header_size = ctypes.sizeof(ServerTypes.sPacketHeader)
+        if len(request_data) < header_size:
+            return
+
+        # Parse the header via ctypes
+        header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size])
+
+        # Handle Connection Handshake
+        if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT):
+            client_requested_version = self.natnet_version  # Fallback to server's version. Version handshaking not supported in this extension.
+
+            client_ip, client_port = client_address
+
+            # Create and store a new client object
+            new_client = Client(client_ip, client_port, version=client_requested_version)
+            try:
+                with self.clients_lock:
+                    self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port
+                    self.connected_clients.add(new_client) # Add the new client to the connected clients list
+                    print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.")
+            except Exception as e:
+                print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}")
+                return
+
+            # Send the server description to the client
+            server_description = self.server_description.pack()
+            response_header = ServerTypes.sPacketHeader(iMessage=int(ServerTypes.MessageId.NAT_SERVERINFO), nDataBytes=len(server_description))
+            
+            # Pack and send the server description to the client
+            response_packet = response_header.pack() + server_description
+            try:
+                with new_client.socket_lock:
+                    self.command_socket.sendto(response_packet, client_address)
+            except Exception as e:
+                raise ValueError(f"[Command Handler] Error sending server description to client {client_address}: {e}")
+            print(f"[Command Handler] Sent server description to client address through its port {client_address}.")
+        
+

From 2f3ddfcf45bd0977ff4c5d25d2767d6267c7c6c0 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Thu, 4 Jun 2026 15:38:55 -0400
Subject: [PATCH 02/27] optitrack emulator isaac-sim boilerplate

---
 .../optitrack.natnet.emulator/.gitignore      | 16 +++++++
 .../NatNetClientSDK/README.md                 |  9 ++++
 .../optitrack.natnet.emulator/README.md       | 46 +++++++++++++++++++
 .../config/extension.toml                     | 15 ++++++
 .../optitrack/__init__.py                     |  1 +
 .../optitrack/natnet/__init__.py              |  1 +
 .../optitrack/natnet/emulator/__init__.py     | 10 ++++
 .../natnet/emulator/server/__init__.py        | 11 +++++
 .../optitrack.natnet.emulator/setup.py        | 20 ++++++++
 9 files changed, 129 insertions(+)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
new file mode 100644
index 000000000..3f6e890cd
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
@@ -0,0 +1,16 @@
+# OptiTrack SDK archives and build artifacts (reference tree may exist locally)
+NatNetClientSDK/*.zip
+NatNetClientSDK/**/x64/
+NatNetClientSDK/**/Release/
+NatNetClientSDK/**/Debug/
+**/*.obj
+**/*.pdb
+**/*.exe
+**/*.iobj
+**/*.ipdb
+**/*.tlog/
+**/__pycache__/
+**/*.pyc
+*.egg-info/
+dist/
+build/
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
new file mode 100644
index 000000000..38d3112cb
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
@@ -0,0 +1,9 @@
+# NatNetClientSDK (reference only)
+
+Local copy of the OptiTrack NatNet 4.4 SDK samples and headers for **protocol development**.
+
+- **Not** installed by the Isaac extension wheel
+- **Not** redistributed by AirStack (OptiTrack license applies)
+- Use for wire-format reference: `NatNetSDK/include/NatNetTypes.h`, `Samples/PacketClient/PacketClient.cpp`
+
+Production robot client uses `libNatNet.so` via `airstack setup --natnet` in [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/).
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
new file mode 100644
index 000000000..aff90f02b
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
@@ -0,0 +1,46 @@
+# OptiTrack NatNet Emulator (Isaac Sim Extension)
+
+Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/).
+
+## Layout
+
+```
+optitrack.natnet.emulator/
+├── config/extension.toml       # Isaac Sim extension manifest (stub)
+├── setup.py
+├── optitrack/natnet/emulator/
+│   └── server/                 # NatNet UDP server (transport + protocol)
+│       ├── natnet_server.py
+│       ├── natnet_unicast_server.py
+│       ├── natnet_data_types.py
+│       └── natnet_server_types.py
+└── NatNetClientSDK/            # Reference SDK only (not shipped in wheel)
+```
+
+## Responsibilities
+
+| Layer | Role |
+|-------|------|
+| **Server** (`optitrack.natnet.emulator.server`) | UDP transport, `NAT_CONNECT` / `NAT_SERVERINFO`, frame relay via `enqueue_mocap_data()` |
+| **Isaac wrapper** (planned) | Build `sFrameOfMocapData` from sim poses; register rigid-body model catalog |
+
+## Usage (development)
+
+```python
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
+
+server = NatNetUnicastServer(local_interface="172.31.0.200")
+server.start()
+# Wrapper enqueues pre-built frames:
+# server.enqueue_mocap_data(frame)
+```
+
+Default Docker sim IP: `172.31.0.200` (Isaac container on AirStack bridge network).
+
+## Protocol notes
+
+For libNatNet 4.4 **unicast**, the client uses a single UDP socket; server replies and frames go to the client's `NAT_CONNECT` source endpoint on the **command port** (1510). See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md).
+
+## Reference material
+
+Wire-format reference lives in [`NatNetClientSDK/`](NatNetClientSDK/README.md) (OptiTrack SDK samples; not redistributed by AirStack).
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
new file mode 100644
index 000000000..b56f9d079
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
@@ -0,0 +1,15 @@
+[package]
+version = "0.1.0"
+title = "OptiTrack NatNet Emulator"
+description = "NatNet UDP server emulator for Isaac Sim integration with natnet_ros2"
+category = "Simulation"
+keywords = ["optitrack", "natnet", "mocap", "simulation"]
+
+[dependencies]
+"omni.isaac.core" = {}
+
+[[python.module]]
+name = "optitrack.natnet.emulator"
+
+[python.build-system]
+requires = ["setuptools"]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py
new file mode 100644
index 000000000..39ed38144
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py
@@ -0,0 +1 @@
+"""OptiTrack NatNet packages for AirStack Isaac Sim integration."""
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py
new file mode 100644
index 000000000..b19da2cbc
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py
@@ -0,0 +1 @@
+"""NatNet simulation components."""
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
new file mode 100644
index 000000000..20e405b79
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
@@ -0,0 +1,10 @@
+"""OptiTrack Motive NatNet emulator for Isaac Sim."""
+
+from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType
+
+__all__ = [
+    "Client",
+    "NatNetServer",
+    "NatNetUnicastServer",
+    "TransmissionType",
+]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py
new file mode 100644
index 000000000..c84c1fe4b
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py
@@ -0,0 +1,11 @@
+"""NatNet UDP server implementation (unicast; multicast planned)."""
+
+from .natnet_server import Client, NatNetServer, TransmissionType
+from .natnet_unicast_server import NatNetUnicastServer
+
+__all__ = [
+    "Client",
+    "NatNetServer",
+    "NatNetUnicastServer",
+    "TransmissionType",
+]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py
new file mode 100644
index 000000000..4bf6d4f70
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py
@@ -0,0 +1,20 @@
+"""Isaac Sim extension install metadata for the OptiTrack NatNet emulator."""
+
+import os
+
+from setuptools import find_packages, setup
+
+EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__))
+
+setup(
+    name="optitrack-natnet-emulator",
+    version="0.1.0",
+    description="NatNet UDP server emulator for Isaac Sim and natnet_ros2 integration",
+    license="MIT",
+    include_package_data=True,
+    python_requires=">=3.10",
+    install_requires=[],
+    packages=find_packages(where="."),
+    package_dir={"": "."},
+    zip_safe=False,
+)

From e9cfc79c94269873f4092da69bbb0e142b9e8015 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Thu, 4 Jun 2026 15:39:19 -0400
Subject: [PATCH 03/27] current documentation update

---
 .agents/skills/optitrack-development/SKILL.md | 201 ++++++++++++++++++
 AGENTS.md                                     |   1 +
 2 files changed, 202 insertions(+)
 create mode 100644 .agents/skills/optitrack-development/SKILL.md

diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md
new file mode 100644
index 000000000..42a561830
--- /dev/null
+++ b/.agents/skills/optitrack-development/SKILL.md
@@ -0,0 +1,201 @@
+---
+name: optitrack-development
+description: Develop and integrate OptiTrack NatNet in AirStack — robot client (natnet_ros2), Isaac Sim Motive emulator, wire-protocol handshake, and libNatNet 4.4 unicast behavior. Use when working on natnet_ros2, optitrack.natnet.emulator, LAUNCH_NATNET, or NatNet UDP protocol compatibility.
+license: Apache-2.0
+metadata:
+  author: AirLab CMU
+  repository: AirStack
+---
+
+# Skill: OptiTrack / NatNet Development
+
+## When to Use
+
+- Implementing or debugging the **Motive emulator** in Isaac Sim
+  (`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`)
+- Integrating or testing **`natnet_ros2`** on the robot stack
+- Understanding **NatNet wire protocol** (connect, model def, frame streaming)
+- Capturing what **`libNatNet.so`** actually sends on the network
+- Enabling OptiTrack in sim: `LAUNCH_NATNET=true`, `natnet_config.yaml`, Docker IPs
+
+## Architecture in AirStack
+
+```mermaid
+flowchart LR
+    subgraph sim ["Isaac Sim (172.31.0.200)"]
+        Emulator["optitrack.natnet.emulator\n(NatNet UDP server)"]
+    end
+    subgraph robot ["Robot container"]
+        Node["natnet_ros2_node"]
+        SDK["libNatNet.so client"]
+        Node --> SDK
+    end
+    SDK -->|"UDP 1510 (unicast: cmd + frames)"| Emulator
+    Node --> Topics["/{ROBOT_NAME}/perception/optitrack/..."]
+```
+
+| Component | Path | Role |
+|-----------|------|------|
+| Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) |
+| SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) |
+| Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests |
+| Planned integration tests | [`tests/sim/motive_emulator/README.md`](../../../tests/sim/motive_emulator/README.md) | End-to-end UDP tests against real SDK parser |
+
+**Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`.
+
+**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511 — see [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
+
+## NatNet: Two UDP Channels
+
+| Port (server default) | Channel | Direction |
+|----------------------|---------|-----------|
+| **1510** | Command | Client → server: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives. Server → client: `NAT_SERVERINFO`, `NAT_MODELDEF`, `NAT_RESPONSE` |
+| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **libNatNet 4.4 unicast clients receive frames on the same socket/port as command traffic** (see below). |
+
+**Critical rules (verified against libNatNet 4.4 unicast):**
+
+- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`).
+- **libNatNet 4.4 unicast uses one client UDP socket** — command send, command receive, and frame receive all share the **same ephemeral local port**. Do **not** assume `data_port = cmd_port + 1`.
+- The **269-byte `NAT_CONNECT` payload does not include** the client port; the port is learned from the datagram **source address** on `NAT_CONNECT`.
+- Do **not** trust `/proc`/`ss` alone for the client port — extra bound sockets may appear that do not match wire traffic. **`NAT_CONNECT` source `(ip, port)` is ground truth.**
+- Do **not** parse connect payloads with in-memory `sNatNetClientConnectParams` (contains pointers). Use on-wire layouts below.
+
+## libNatNet 4.4 `NAT_CONNECT` (verified 2025-06)
+
+Observed against `127.0.0.1:1510` with the same unicast params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp).
+
+### What the client sends
+
+| Field | Observed value |
+|-------|----------------|
+| Message | `NAT_CONNECT` (0), `nDataBytes = 269`, total datagram 273 bytes |
+| Payload layout | `sSender` (264 B) + `sConnectionOptions` (5 B) |
+| `sSender.szName` | `"NatNetLib"` |
+| `sSender.Version` | `[4, 4, 0, 0]` |
+| `sSender.NatNetVersion` | `[4, 4, 0, 0]` |
+| `subscribedDataOnly` | `0` |
+| `BitstreamVersion` | `[0, 0, 0, 0]` → client defers to server version |
+| Trailing port bytes | **None** (exactly 269 bytes; not PacketClient's optional +4) |
+| UDP source port | Ephemeral (e.g. `41449`) — **client command + data port (same socket)** |
+
+Example hex (payload only, after 4-byte header):
+
+```
+NatNetLib\0 ... (256-byte name field)
+04 04 00 00  (Version)
+04 04 00 00  (NatNetVersion)
+00           (subscribedDataOnly)
+00 00 00 00  (BitstreamVersion)
+```
+
+## libNatNet 4.4 unicast: single client socket (verified 2025-06)
+
+Confirmed with wire capture on server `:1510`/`:1511`, `strace` on a minimal `NatNetClient::Connect()` binary, and `/proc/<pid>/net/udp` cross-checks against the same `libNatNet.so` used by `natnet_ros2`.
+
+### What we observed
+
+| Signal | Result |
+|--------|--------|
+| Wire capture on server `:1510` | All client packets (`NAT_CONNECT`, `NAT_KEEPALIVE`, `NAT_REQUEST_MODELDEF`) from **one** source port |
+| Wire capture on server `:1511` | **No** inbound packets from the client |
+| strace on minimal client | **One** `bind()`, **one** fd for all `sendto` → server `:1510` and `recvfrom` ← server `:1510` |
+| `NAT_CONNECT` payload | **No** trailing client port bytes (269 B total) |
+
+### Emulator rule (unicast + `natnet_ros2`)
+
+For libNatNet 4.4 unicast, treat the client as **single-endpoint**:
+
+```text
+On NAT_CONNECT     → store client_endpoint = (ip, port) from recvfrom
+NAT_SERVERINFO     → sendto(command_socket, client_endpoint)
+NAT_MODELDEF       → sendto(command_socket, client_endpoint)
+NAT_FRAMEOFDATA    → sendto(command_socket, client_endpoint)   # same port, not cmd+1
+NAT_KEEPALIVE ack  → sendto(command_socket, client_endpoint)
+```
+
+The server still **binds** command (`1510`) and data (`1511`) ports per NatNet convention, but **`natnet_ros2` does not expose a separate client data port** — stream frames to the **`NAT_CONNECT` source address** from the **command socket**.
+
+`ConnectionDataPort = 1511` in `NAT_SERVERINFO` remains required (SDK expects it); it describes the server's data port, not a second client listener in this mode.
+
+### When two client ports may still apply
+
+- **Multicast** clients (separate multicast data listener on `239.255.42.99:1511`)
+- **PacketClient-style** samples that open explicit command + data sockets (optional +4 port bytes in connect)
+- Other NatNet client implementations — always verify with protocol capture before assuming a two-socket model
+
+Do **not** assume `data_port = cmd_port + 1` for any client without capture.
+
+### What the server must reply (for `Connect()` + `GetServerDescription()`)
+
+1. **`NAT_SERVERINFO` (1)** on the **command port** to the connect datagram source.
+2. Payload: full packed **`sServerDescription`** ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)), including:
+   - `HostPresent = true`
+   - `szHostApp = "Motive"`
+   - `NatNetVersion = {4, 4, 0, 0}`
+   - `bConnectionInfoValid = true`, `ConnectionDataPort = 1511`, `ConnectionMulticast = false` (unicast)
+
+Pre-built in emulator: [`NatNetServer._build_server_description()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py).
+
+### After connect (required for `natnet_ros2` topics)
+
+| SDK call | Server must handle |
+|----------|-------------------|
+| `GetDataDescriptionList()` | `NAT_REQUEST_MODELDEF` → `NAT_MODELDEF` with rigid body name/ID (e.g. `"Drone"`) |
+| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** via server command socket; set `rb.params & 0x01` (tracking valid) |
+| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; reply on same client endpoint |
+
+Frame delivery to the client endpoint still needs a valid `NAT_FRAMEOFDATA` serializer in the emulator; the **single-socket client model** above is confirmed via strace + wire capture.
+
+## Wire format reference (do not confuse)
+
+| Client type | Connect payload |
+|-------------|-----------------|
+| **`libNatNet` / `natnet_ros2`** | `sSender` + `sConnectionOptions` (269 B observed) |
+| **PacketClient sample** | Same + optional 4 trailing bytes (often zero in sample) |
+| **Python NatNetClient sample** | Legacy 270-byte `"Ping"` blob — **not** used by `natnet_ros2` |
+
+API struct `sNatNetClientConnectParams` ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)) is for `Connect()` in process memory only — **not** the on-wire layout.
+
+## Protocol capture (optional, for debugging)
+
+Not part of the repo. If you need to re-verify wire behavior or debug a new client/server pairing, build a **minimal out-of-band harness**:
+
+1. **Minimal C++ client** — tiny binary linking `libNatNet.so` from `natnet_ros2`; call `NatNetClient::Connect()` with the same params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). Optional: `GetDataDescriptionList()`, frame callback, `--hold-seconds` sleep.
+2. **Python UDP stub server** — bind `:1510` (and optionally `:1511`); reply to `NAT_CONNECT` with canned `NAT_SERVERINFO`, to `NAT_REQUEST_MODELDEF` with `NAT_MODELDEF`, to `NAT_KEEPALIVE` with ack; log every `(ip, port)` and message id.
+3. **Connect capture** — run the client against the stub; hex-dump the first datagram; confirm 269-byte `sSender` + `sConnectionOptions` payload and ephemeral source port.
+4. **Endpoint discovery** — during a full connect + model-def fetch:
+   - `tcpdump -i any udp and host <client_ip>` or the stub's packet log
+   - `strace -e trace=bind,sendto,recvfrom` on the client binary
+   - `/proc/<pid>/net/udp` or `ss -uapn` (treat **`NAT_CONNECT` source port** as ground truth if they disagree)
+5. **Frame delivery check** — once the emulator can emit valid `NAT_FRAMEOFDATA`, confirm the client's frame callback fires when sending to the `NAT_CONNECT` source endpoint from the server command socket.
+
+Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not in-memory `sNatNetClientConnectParams`.
+
+## Emulator implementation checklist
+
+1. **Command listener** on `0.0.0.0:1510`
+2. **`NAT_CONNECT`** → register `client_endpoint` from `recvfrom`; reply `NAT_SERVERINFO`
+3. **`NAT_REQUEST_MODELDEF`** → reply `NAT_MODELDEF` (match `body_name` in config)
+4. **Frame loop** → `NAT_FRAMEOFDATA` to **`client_endpoint` via command socket** (libNatNet 4.4 unicast — same port as connect, not `cmd+1`)
+5. **Isaac integration** → sample drone pose → `sFrameOfMocapData` → `enqueue_mocap_data()`
+6. **Docker** → emulator on `172.31.0.200`; robot `server_ip` points there
+
+## Testing levels
+
+| Level | Approach | Validates |
+|-------|----------|-----------|
+| Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names |
+| Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model |
+| Integration (planned) | `tests/sim/motive_emulator/` | Full SDK parser + `natnet_ros2_node` |
+| System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` |
+
+```bash
+# Unit tests (robot container)
+docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+"
+```
+
+## References
+
+- OptiTrack NatNet docs: https://docs.optitrack.com/developer-tools/natnet-sdk/natnet-4.0
+- SDK samples (wire format): `NatNet_SDK_*/Samples/PacketClient/`, `PythonClient/` (legacy connect in Python only)
+- Integration test plan: [`tests/sim/motive_emulator/README.md`](../../../tests/sim/motive_emulator/README.md)
diff --git a/AGENTS.md b/AGENTS.md
index b53069e75..6bb05ad0a 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -89,6 +89,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc
 | [write-launch-file](.agents/skills/write-launch-file) | Authoring ROS 2 launch files with AirStack conventions (ROBOT_NAME namespacing, topic remapping, allow_substs) |
 | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes |
 | [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS |
+| [optitrack-development](.agents/skills/optitrack-development) | OptiTrack NatNet emulator, natnet_ros2, wire-protocol handshake, connect-packet sniff |
 | [attach-gossip-payload](.agents/skills/attach-gossip-payload) | Broadcasting custom ROS messages to peers via PeerProfile gossip payloads |
 | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules |
 | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs |

From e9e5c7e09d93445f800e647baae9d9ebb0ae7e69 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Thu, 4 Jun 2026 15:46:43 -0400
Subject: [PATCH 04/27] bugfix applied to assigning hostname for server on
 startup

---
 .../natnet/emulator/server/natnet_server.py   | 572 +++++++++---------
 .../emulator/server/natnet_unicast_server.py  | 211 +++----
 2 files changed, 409 insertions(+), 374 deletions(-)

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
index e5e4ef7bd..a7d68db87 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
@@ -1,270 +1,302 @@
-from typing import Any
-
-
-from . import natnet_data_types as DataMessages
-from . import natnet_server_types as ServerMessages
-from enum import Enum
-import socket
-import threading
-import queue
-import time
-import signal
-import ctypes
-import typing
-
-
-class TransmissionType(str, Enum):
-    UNICAST = "unicast"
-    MULTICAST = "multicast"
-
-class Client:
-    def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)):
-        self.ip = ip
-        self.port = port
-        self.version = version
-        self.subscribed_assets = set()
-        self.socket_lock = threading.Lock()
-
-    def __hash__(self):
-        # We uniquely identify a client session by their IP and their unique command port.
-        # Why? A single machine could technically run multiple separate NatNet clients
-        # simultaneously, and they would share an IP but have unique command ports.
-        return hash((self.ip, self.port))
-
-    def __eq__(self, other):
-        return (isinstance(other, Client) and 
-                self.ip == other.ip and 
-                self.port == other.port)
-    
-
-class NatNetServer:
-    def __init__(self, 
-            local_interface : str = "172.31.0.200",
-            transmission_type: TransmissionType = TransmissionType.MULTICAST, 
-            multicast_address : str = "239.255.42.99",
-            command_port: int = 1510,
-            data_port : int = 1511,
-            motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0),
-            natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0),
-            high_res_clock_freq : int = 1_000_000_000, 
-            publish_rate : int = 100 # Hz (default 100Hz)
-        ):
-
-        self.local_interface = local_interface
-        self.transmission_type = transmission_type
-        self.multicast_address = multicast_address
-        self.command_port = command_port
-        self.data_port = data_port
-        self.motive_app_version = motive_app_version
-        self.natnet_version = natnet_version
-        self.high_res_clock_freq = high_res_clock_freq
-        self.publish_rate = publish_rate
-        
-        self._validate_init_params()
-
-        self.server_description = self._build_server_description()
-        # Initialize synchronously safe data structures for server state and mocap data
-
-        # Thread-safe queue for Mocp frames
-        self.mocap_data_queue = queue.Queue(maxsize=100)
-        
-        # Thread list and shutdown event
-        self.threads = []
-        self.shutdown_event = threading.Event()
-
-        # Connected clients for unicast mode
-        self.connected_clients : typing.Set[Client] = set()
-        self.clients_lock : threading.Lock = threading.Lock()
-
-        # Sockets
-        self.command_socket : socket.socket | None = None
-        self.data_socket : socket.socket | None = None
-
-        self.running = False
-
-        # Start up process with threads
-        # # One thread to listen and manage command requests (TCP/UDP)
-        # # One thread to send data packets (UDP)
-        # # One thread to manage server state and data updates (mocap system or other source)
-
-        # Overall design:
-        # - On initialization, validate parameters and set up server description
-        # - Start threads for command listening and data sending
-        # - Perhaps separate into Unicast and Multicast server classes that inherit from a common base, or handle both in one class with conditional logic based on transmission type
-        # - Multicast server will need to join multicast group and manage socket options accordingly, while unicast server will send directly to client IPs
-        # - General helper functions will break packets down into appropriate sizes, serialize data messages, and manage client connections for unicast case
-        # - General main loop will manage server lifecycle and clean shutdown, while threads will handle their respective tasks for command listening and data sending
-        # - General main data management function will take in new mocap data (e.g. from mocap system or other source), update the latest data state, and trigger packet sending to clients at regular intervals (e.g. 100Hz)
-        # - Unicast:
-        # - On command request, respond with server description and data packets sent directly to requesting client's IP
-        # - Have a synchronously safe list of connected clients to manage multiple unicast recipients
-        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
-        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to unicast clients.
-        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets 
-        # - Multicast:
-        # - On command request, respond with server description but data packets will be sent to the multicast group address rather than directly to client IPs
-        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
-        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to the multicast group address.
-        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets and send to the multicast group address.
-
-    def _signal_handler(self, signum, frame):
-        print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...")
-        self.shutdown()
-
-    def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData):
-        # Thread-safe method to push new physics frames (called by Isaac-Sim extension)
-        if self.mocap_data_queue.full():
-            try:
-                # Drop oldest frame if falling behind
-                self.mocap_data_queue.get_nowait()
-            except queue.Empty:
-                pass
-        self.mocap_data_queue.put(new_data)
-
-    def start(self):
-        # Bind sockets and launch worker threads automatically on init
-        
-        # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill)
-        try:
-            signal.signal(signal.SIGINT, self._signal_handler)
-            signal.signal(signal.SIGTERM, self._signal_handler)
-        except ValueError:
-            pass # Safe fallback if not called from the main thread
-        
-        # 1. Setup Command Socket (Receives connection/discovery requests)
-        self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
-        self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
-        self.command_socket.bind(('', self.command_port))
-
-        # 2. Setup Data Socket (Sends outward Mocap frames)
-        self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
-        # Note: Data socket doesn't need to bind to the multicast explicitly if it's only sending.
-        # It just routes via the local interface.
-        self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface))
-
-        # 3. Launch Threads
-        cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True)
-        data_thread = threading.Thread(target=self._data_update_loop, daemon=True)
-
-        self.threads.extend([cmd_thread, data_thread])
-        
-        for t in self.threads:
-            t.start()
-        
-        self.running = True
-
-    def shutdown(self):
-        # Cleanly shutdown threads and close sockets
-        self.running = False
-        self.shutdown_event.set()
-        
-        if self.command_socket:
-            self.command_socket.close()
-            
-        if self.data_socket:
-            self.data_socket.close()
-
-        for t in self.threads:
-            if t.is_alive():
-                t.join(timeout=1.0)
-
-    def _validate_init_params(self):
-
-        # Validate the local_interface is a valid IP address
-        if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3:
-            raise ValueError(f"Invalid local interface IP address: {self.local_interface}")
-
-        # Validate between transmission types and address requirements
-        if self.transmission_type not in TransmissionType:
-            raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.")
-
-        if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address:
-            raise ValueError("Multicast address must be provided for multicast transmission type.")
-        
-        if self.transmission_type == TransmissionType.UNICAST and self.multicast_address:
-            raise ValueError("Multicast address should not be provided for unicast transmission type.")
-
-        if not (0 < self.command_port < 65536):
-            raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.")
-
-        if not (0 < self.data_port < 65536):
-            raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.")
-        
-        if self.command_port == self.data_port:
-            raise ValueError("Command port and data port must be different.")
-
-        if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4):
-            raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
-
-        if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4):
-            raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
-        
-        if self.motive_app_version and not self.motive_app_version[0] == 3:
-            raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0")
-
-        if not self.natnet_version[0] == 4:
-            raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0")
-
-        if self.high_res_clock_freq <= 0:
-            raise ValueError(f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz.")
-
-    def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData:
-        # Thread-safe method to retrieve the latest mocap data to be sent
-        try:
-            return self.mocap_data_queue.get_nowait()
-        except queue.Empty:
-            return None
-
-    def _build_server_description(self) -> ServerMessages.sServerDescription:
-        # Helper to build the server description struct with current server info (e.g. on startup or in response to command request)
-        description = ServerMessages.sServerDescription()
-        description.HostPresent = True
-        description.szHostComputerName = socket.gethostname().encode('utf-8')[:ServerMessages.MAX_NAMELENGTH-1] + b'\x00'
-        description.HostComputerAddress = socket.inet_aton(self.local_interface)
-        description.szHostApp = b'Motive'
-        description.HostAppVersion = self.motive_app_version
-        description.NatNetVersion = self.natnet_version
-        description.HighResClockFrequency = self.high_res_clock_freq
-        description.bConnectionInfoValid = True
-        description.ConnectionDataPort = self.data_port
-        description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST
-        
-        if self.transmission_type == TransmissionType.MULTICAST:
-            description.ConnectionMulticastAddress = socket.inet_aton(self.multicast_address)
-        else:
-            description.ConnectionMulticastAddress = b'\x00\x00\x00\x00' # Setting to 0 explicitly for unicast case
-        
-        return description
-
-    def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        # Loop to update mocap data and send packets at regular intervals (e.g. 100Hz)
-        pass
-
-    def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData):
-        # Serialize data_messages and send as UDP packet to client or multicast group
-        
-        if not self.data_socket:
-            raise ValueError("[NatNetServer] Data socket not initialized. Cannot send data packet.")
-
-        # Serialize the data message into bytes
-        try:
-            packet_bytes = data_message.pack()
-            header = ServerMessages.sPacketHeader(iMessage=ServerMessages.MessageId.NAT_FRAMEOFDATA, nDataBytes=len(packet_bytes))
-            full_packet = header.pack() + packet_bytes
-        except Exception as e:
-            raise ValueError(f"[NatNetServer] Error serializing data message: {e}")
-        
-        try:
-            with client.socket_lock:
-                self.data_socket.sendto(full_packet, (client.ip, client.port))
-        except Exception as e:
-            raise ValueError(f"[NatNetServer] Error sending data packet to client {client.ip}:{client.port}: {e}")
-
-    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        # Loop to listen for and handle incoming command requests (e.g. from client apps)
-        pass
-
-    def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        # Parse incoming command request, perform requested action, and send response if needed
-        pass
-
+from . import natnet_data_types as DataMessages
+from . import natnet_server_types as ServerMessages
+from enum import Enum
+import socket
+import threading
+import queue
+import signal
+import ctypes
+import typing
+
+
+class TransmissionType(str, Enum):
+    UNICAST = "unicast"
+    MULTICAST = "multicast"
+
+class Client:
+    def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)):
+        self.ip = ip
+        self.port = port
+        self.version = version
+        self.subscribed_assets = set()
+        self.socket_lock = threading.Lock()
+
+    def __hash__(self):
+        # We uniquely identify a client session by their IP and their unique command port.
+        # Why? A single machine could technically run multiple separate NatNet clients
+        # simultaneously, and they would share an IP but have unique command ports.
+        return hash((self.ip, self.port))
+
+    def __eq__(self, other):
+        return (isinstance(other, Client) and 
+                self.ip == other.ip and 
+                self.port == other.port)
+    
+
+class NatNetServer:
+    def __init__(self, 
+            local_interface : str = "172.31.0.200",
+            transmission_type: TransmissionType = TransmissionType.MULTICAST, 
+            multicast_address : str = "239.255.42.99",
+            command_port: int = 1510,
+            data_port : int = 1511,
+            motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0),
+            natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0),
+            high_res_clock_freq : int = 1_000_000_000, 
+            publish_rate : int = 100 # Hz (default 100Hz)
+        ):
+
+        self.local_interface = local_interface
+        self.transmission_type = transmission_type
+        self.multicast_address = multicast_address
+        self.command_port = command_port
+        self.data_port = data_port
+        self.motive_app_version = motive_app_version
+        self.natnet_version = natnet_version
+        self.high_res_clock_freq = high_res_clock_freq
+        self.publish_rate = publish_rate
+        
+        self._validate_init_params()
+
+        self.server_description = self._build_server_description()
+        # Initialize synchronously safe data structures for server state and mocap data
+
+        # Thread-safe queue for Mocp frames
+        self.mocap_data_queue = queue.Queue(maxsize=100)
+        
+        # Thread list and shutdown event
+        self.threads = []
+        self.shutdown_event = threading.Event()
+
+        # Connected clients for unicast mode
+        self.connected_clients : typing.Set[Client] = set()
+        self.clients_lock : threading.Lock = threading.Lock()
+
+        # Sockets
+        self.command_socket : socket.socket | None = None
+        self.data_socket : socket.socket | None = None
+
+        self.running = False
+
+        # Start up process with threads
+        # # One thread to listen and manage command requests (TCP/UDP)
+        # # One thread to send data packets (UDP)
+        # # One thread to manage server state and data updates (mocap system or other source)
+
+        # Overall design:
+        # - On initialization, validate parameters and set up server description
+        # - Start threads for command listening and data sending
+        # - Perhaps separate into Unicast and Multicast server classes that inherit from a common base, or handle both in one class with conditional logic based on transmission type
+        # - Multicast server will need to join multicast group and manage socket options accordingly, while unicast server will send directly to client IPs
+        # - General helper functions will break packets down into appropriate sizes, serialize data messages, and manage client connections for unicast case
+        # - General main loop will manage server lifecycle and clean shutdown, while threads will handle their respective tasks for command listening and data sending
+        # - General main data management function will take in new mocap data (e.g. from mocap system or other source), update the latest data state, and trigger packet sending to clients at regular intervals (e.g. 100Hz)
+        # - Unicast:
+        # - On command request, respond with server description and data packets sent directly to requesting client's IP
+        # - Have a synchronously safe list of connected clients to manage multiple unicast recipients
+        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
+        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to unicast clients.
+        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets 
+        # - Multicast:
+        # - On command request, respond with server description but data packets will be sent to the multicast group address rather than directly to client IPs
+        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
+        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to the multicast group address.
+        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets and send to the multicast group address.
+
+    def _signal_handler(self, signum, frame):
+        print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...")
+        self.shutdown()
+
+    def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData):
+        # Thread-safe method to push new physics frames (called by Isaac-Sim extension)
+        if self.mocap_data_queue.full():
+            try:
+                # Drop oldest frame if falling behind
+                self.mocap_data_queue.get_nowait()
+            except queue.Empty:
+                pass
+        self.mocap_data_queue.put(new_data)
+
+    def start(self):
+        # Bind sockets and launch worker threads automatically on init
+        
+        # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill)
+        try:
+            signal.signal(signal.SIGINT, self._signal_handler)
+            signal.signal(signal.SIGTERM, self._signal_handler)
+        except ValueError:
+            pass # Safe fallback if not called from the main thread
+        
+        # 1. Setup Command Socket (Receives connection/discovery requests)
+        self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+        self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self.command_socket.bind(('', self.command_port))
+
+        # 2. Setup Data Socket (Sends outward Mocap frames)
+        self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+        # Note: Data socket doesn't need to bind to the multicast explicitly if it's only sending.
+        # It just routes via the local interface.
+        self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface))
+
+        # 3. Launch Threads
+        cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True)
+        data_thread = threading.Thread(target=self._data_update_loop, daemon=True)
+
+        self.threads.extend([cmd_thread, data_thread])
+        
+        for t in self.threads:
+            t.start()
+        
+        self.running = True
+
+    def shutdown(self):
+        # Cleanly shutdown threads and close sockets
+        self.running = False
+        self.shutdown_event.set()
+        
+        if self.command_socket:
+            self.command_socket.close()
+            
+        if self.data_socket:
+            self.data_socket.close()
+
+        for t in self.threads:
+            if t.is_alive():
+                t.join(timeout=1.0)
+
+    def _validate_init_params(self):
+
+        # Validate the local_interface is a valid IP address
+        if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3:
+            raise ValueError(f"Invalid local interface IP address: {self.local_interface}")
+
+        # Validate between transmission types and address requirements
+        if self.transmission_type not in TransmissionType:
+            raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.")
+
+        if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address:
+            raise ValueError("Multicast address must be provided for multicast transmission type.")
+        
+        if self.transmission_type == TransmissionType.UNICAST and self.multicast_address:
+            raise ValueError("Multicast address should not be provided for unicast transmission type.")
+
+        if not (0 < self.command_port < 65536):
+            raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.")
+
+        if not (0 < self.data_port < 65536):
+            raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.")
+        
+        if self.command_port == self.data_port:
+            raise ValueError("Command port and data port must be different.")
+
+        if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4):
+            raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
+
+        if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4):
+            raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).")
+        
+        if self.motive_app_version and not self.motive_app_version[0] == 3:
+            raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0")
+
+        if not self.natnet_version[0] == 4:
+            raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0")
+
+        if self.high_res_clock_freq <= 0:
+            raise ValueError(f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz.")
+
+    def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None:
+        # Thread-safe method to retrieve the latest mocap data to be sent
+        try:
+            return self.mocap_data_queue.get_nowait()
+        except queue.Empty:
+            return None
+
+    @staticmethod
+    def _pad_fixed_string(value: bytes) -> bytes:
+        """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields."""
+        truncated = value[: ServerMessages.MAX_NAMELENGTH - 1]
+        return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated))
+
+    @staticmethod
+    def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None:
+        for index, component in enumerate(version):
+            field[index] = component
+
+    @staticmethod
+    def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None:
+        octets = socket.inet_aton(address) if isinstance(address, str) else address
+        for index, octet in enumerate(octets):
+            field[index] = octet
+
+    def _build_server_description(self) -> ServerMessages.sServerDescription:
+        # Helper to build the server description struct with current server info (e.g. on startup or in response to command request)
+        description = ServerMessages.sServerDescription()
+        description.HostPresent = True
+        description.szHostComputerName = self._pad_fixed_string(
+            socket.gethostname().encode("utf-8")
+        )
+        self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface)
+        description.szHostApp = self._pad_fixed_string(b"Motive")
+        self._assign_version_bytes(description.HostAppVersion, self.motive_app_version)
+        self._assign_version_bytes(description.NatNetVersion, self.natnet_version)
+        description.HighResClockFrequency = self.high_res_clock_freq
+        description.bConnectionInfoValid = True
+        description.ConnectionDataPort = self.data_port
+        description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST
+
+        if self.transmission_type == TransmissionType.MULTICAST:
+            self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address)
+        else:
+            self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00")
+
+        return description
+
+    def _send_packet_to_client(
+        self,
+        client: Client,
+        message_id: ServerMessages.MessageId | int,
+        payload: bytes,
+    ) -> None:
+        """Send a NatNet packet to a unicast client on the command socket (libNatNet 4.4)."""
+        if not self.command_socket:
+            raise ValueError("[NatNetServer] Command socket not initialized. Cannot send packet.")
+
+        header = ServerMessages.sPacketHeader(
+            iMessage=int(message_id),
+            nDataBytes=len(payload),
+        )
+        packet = header.pack() + payload
+        try:
+            with client.socket_lock:
+                self.command_socket.sendto(packet, (client.ip, client.port))
+        except OSError as e:
+            raise ValueError(
+                f"[NatNetServer] Error sending message {int(message_id)} to "
+                f"client {client.ip}:{client.port}: {e}"
+            ) from e
+
+    def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Loop to update mocap data and send packets at regular intervals (e.g. 100Hz)
+        pass
+
+    def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData):
+        # Serialize frame payload and send via command socket (unicast libNatNet 4.4).
+        try:
+            packet_bytes = data_message.pack()
+        except Exception as e:
+            raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e
+
+        self._send_packet_to_client(
+            client,
+            ServerMessages.MessageId.NAT_FRAMEOFDATA,
+            packet_bytes,
+        )
+
+    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Loop to listen for and handle incoming command requests (e.g. from client apps)
+        pass
+
+    def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Parse incoming command request, perform requested action, and send response if needed
+        pass
+
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index a4186a24c..5ed7b3a76 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -1,104 +1,107 @@
-from . import natnet_data_types as DataTypes
-from . import natnet_server_types as ServerTypes
-from enum import Enum
-import socket
-import threading
-import queue
-import time
-import signal
-import ctypes
-from .natnet_server import TransmissionType, Client, NatNetServer
-
-
-class NatNetUnicastServer(NatNetServer):
-    def __init__(self, 
-            local_interface="172.31.0.200",
-            transmission_type: TransmissionType = TransmissionType.UNICAST, 
-            multicast_address=None,
-            command_port=1510,
-            data_port=1511
-        ):
-
-        if not transmission_type == TransmissionType.UNICAST:
-            raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.")
-
-        super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port)
-
-    def _data_update_loop(self):
-        # Loop to update mocap data and send packets at regular intervals
-        while not self.shutdown_event.is_set():
-            time.sleep(1/self.publish_rate) # Sleep for the duration of one publish cycle
-
-            data_messages = self._get_latest_mocap_packet()
-            if not data_messages:
-                continue
-
-            with self.clients_lock:
-                for client in self.connected_clients:
-                    try:
-                        self._send_data_packet(client, data_messages) # Thread-safely sends data packet to all connected clients
-                    except ValueError as e:
-                        print(str(e))
-                        continue
-
-    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        # Listens on UDP command socket for incoming command requests from clients.
-        # Handles incoming client handshakes and teardown.
-
-        print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...")
-
-        while not self.shutdown_event.is_set():
-            try:
-                data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests
-                if not data:
-                    continue
-                print(f"[Command Listener] Received command request from client {addr}.")
-                self._handle_command_request(data, addr)
-            except Exception as e:
-                if self.shutdown_event.is_set():
-                    break
-                print(f"[Command Listener] Error receiving command request: {e}")
-                time.sleep(0.1) # Sleep briefly to avoid tight loop on errors
-    
-    def _handle_command_request(self, request_data: bytes, client_address: tuple): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        """
-        Processes standard binary headers and registers unicast endpoints.
-        """
-        header_size = ctypes.sizeof(ServerTypes.sPacketHeader)
-        if len(request_data) < header_size:
-            return
-
-        # Parse the header via ctypes
-        header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size])
-
-        # Handle Connection Handshake
-        if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT):
-            client_requested_version = self.natnet_version  # Fallback to server's version. Version handshaking not supported in this extension.
-
-            client_ip, client_port = client_address
-
-            # Create and store a new client object
-            new_client = Client(client_ip, client_port, version=client_requested_version)
-            try:
-                with self.clients_lock:
-                    self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port
-                    self.connected_clients.add(new_client) # Add the new client to the connected clients list
-                    print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.")
-            except Exception as e:
-                print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}")
-                return
-
-            # Send the server description to the client
-            server_description = self.server_description.pack()
-            response_header = ServerTypes.sPacketHeader(iMessage=int(ServerTypes.MessageId.NAT_SERVERINFO), nDataBytes=len(server_description))
-            
-            # Pack and send the server description to the client
-            response_packet = response_header.pack() + server_description
-            try:
-                with new_client.socket_lock:
-                    self.command_socket.sendto(response_packet, client_address)
-            except Exception as e:
-                raise ValueError(f"[Command Handler] Error sending server description to client {client_address}: {e}")
-            print(f"[Command Handler] Sent server description to client address through its port {client_address}.")
-        
-
+from . import natnet_data_types as DataTypes
+from . import natnet_server_types as ServerTypes
+from enum import Enum
+import socket
+import threading
+import queue
+import time
+import signal
+import ctypes
+from .natnet_server import TransmissionType, Client, NatNetServer
+
+
+class NatNetUnicastServer(NatNetServer):
+    def __init__(self, 
+            local_interface="172.31.0.200",
+            transmission_type: TransmissionType = TransmissionType.UNICAST, 
+            multicast_address=None,
+            command_port=1510,
+            data_port=1511
+        ):
+
+        if not transmission_type == TransmissionType.UNICAST:
+            raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.")
+
+        super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port)
+
+    def _data_update_loop(self):
+        # Loop to update mocap data and send packets at regular intervals
+        while not self.shutdown_event.is_set():
+            time.sleep(1 / self.publish_rate)
+
+            with self.clients_lock:
+                clients = list(self.connected_clients)
+            if not clients:
+                continue
+
+            data_messages = self._get_latest_mocap_packet()
+            if not data_messages:
+                continue
+
+            for client in clients:
+                try:
+                    self._send_data_packet(client, data_messages)
+                except ValueError as e:
+                    print(str(e))
+                    continue
+
+    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        # Listens on UDP command socket for incoming command requests from clients.
+        # Handles incoming client handshakes and teardown.
+
+        print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...")
+
+        while not self.shutdown_event.is_set():
+            try:
+                data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests
+                if not data:
+                    continue
+                print(f"[Command Listener] Received command request from client {addr}.")
+                self._handle_command_request(data, addr)
+            except Exception as e:
+                if self.shutdown_event.is_set():
+                    break
+                print(f"[Command Listener] Error receiving command request: {e}")
+                time.sleep(0.1) # Sleep briefly to avoid tight loop on errors
+    
+    def _handle_command_request(self, request_data: bytes, client_address: tuple): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+        """
+        Processes standard binary headers and registers unicast endpoints.
+        """
+        header_size = ctypes.sizeof(ServerTypes.sPacketHeader)
+        if len(request_data) < header_size:
+            return
+
+        # Parse the header via ctypes
+        header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size])
+
+        # Handle Connection Handshake
+        if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT):
+            client_requested_version = self.natnet_version  # Fallback to server's version. Version handshaking not supported in this extension.
+
+            client_ip, client_port = client_address
+
+            # Create and store a new client object
+            new_client = Client(client_ip, client_port, version=client_requested_version)
+            try:
+                with self.clients_lock:
+                    self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port
+                    self.connected_clients.add(new_client) # Add the new client to the connected clients list
+                    print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.")
+            except Exception as e:
+                print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}")
+                return
+
+            try:
+                self._send_packet_to_client(
+                    new_client,
+                    ServerTypes.MessageId.NAT_SERVERINFO,
+                    self.server_description.pack(),
+                )
+            except ValueError as e:
+                raise ValueError(
+                    f"[Command Handler] Error sending server description to client {client_address}: {e}"
+                ) from e
+            print(f"[Command Handler] Sent server description to client address through its port {client_address}.")
+        
+

From 394b03daa78ef53168202620cf22cee1d515deb7 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Thu, 4 Jun 2026 16:52:06 -0400
Subject: [PATCH 05/27] first round of unit tests for optitrack connection and
 basic data messages

---
 .../test/natnet_test_helpers.py               |  86 +++++++++++++
 .../test/test_serializers.py                  |  89 ++++++++++++++
 .../test/test_unicast_protocol.py             | 113 ++++++++++++++++++
 tests/conftest.py                             |  12 ++
 .../test_serializers.py                       |  20 ++++
 .../test_unicast_protocol.py                  |  20 ++++
 6 files changed, 340 insertions(+)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_serializers.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
new file mode 100644
index 000000000..d7e12413a
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
@@ -0,0 +1,86 @@
+"""Shared helpers for optitrack.natnet.emulator unit tests."""
+
+from __future__ import annotations
+
+import socket
+import struct
+import sys
+import time
+from contextlib import contextmanager
+from pathlib import Path
+
+_EXT_ROOT = Path(__file__).resolve().parents[1]
+if str(_EXT_ROOT) not in sys.path:
+    sys.path.insert(0, str(_EXT_ROOT))
+
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
+from optitrack.natnet.emulator.server import natnet_server_types as st
+
+
+def ephemeral_udp_port(host: str = "127.0.0.1") -> int:
+    """Return a free UDP port on *host* by binding and releasing a probe socket."""
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe:
+        probe.bind((host, 0))
+        return probe.getsockname()[1]
+
+
+class NatNetTestClient:
+    """Minimal UDP client for NatNet command-port protocol tests."""
+
+    def __init__(self, host: str = "127.0.0.1", timeout: float = 2.0) -> None:
+        self._host = host
+        self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self._sock.bind((host, 0))
+        self._sock.settimeout(timeout)
+
+    @property
+    def local_port(self) -> int:
+        return self._sock.getsockname()[1]
+
+    def send_message(
+        self,
+        server_port: int,
+        message_id: st.MessageId | int,
+        payload: bytes = b"",
+        server_host: str | None = None,
+    ) -> None:
+        header = st.sPacketHeader(
+            iMessage=int(message_id),
+            nDataBytes=len(payload),
+        )
+        self._sock.sendto(
+            header.pack() + payload,
+            (server_host or self._host, server_port),
+        )
+
+    def recv_message(self) -> tuple[int, bytes, tuple[str, int]]:
+        data, addr = self._sock.recvfrom(65535)
+        message_id, payload_len = struct.unpack("<HH", data[:4])
+        payload = data[4 : 4 + payload_len]
+        return message_id, payload, addr
+
+    def close(self) -> None:
+        self._sock.close()
+
+
+@contextmanager
+def running_unicast_server(
+    command_port: int | None = None,
+    local_interface: str = "127.0.0.1",
+    publish_rate: int = 100,
+):
+    """Start NatNetUnicastServer on an ephemeral or fixed command port."""
+    port = command_port if command_port is not None else ephemeral_udp_port(local_interface)
+    server = NatNetUnicastServer(
+        local_interface=local_interface,
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=port,
+    )
+    server.publish_rate = publish_rate
+    server.start()
+    time.sleep(0.05)
+    try:
+        yield server, port
+    finally:
+        server.shutdown()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
new file mode 100644
index 000000000..9ae58b093
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
@@ -0,0 +1,89 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Unit tests for NatNet wire serializers (no network)."""
+
+from __future__ import annotations
+
+import ctypes
+import struct
+import sys
+from pathlib import Path
+
+import pytest
+
+_EXT_ROOT = Path(__file__).resolve().parents[1]
+if str(_EXT_ROOT) not in sys.path:
+    sys.path.insert(0, str(_EXT_ROOT))
+
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
+from optitrack.natnet.emulator.server import natnet_data_types as dt
+from optitrack.natnet.emulator.server import natnet_server_types as st
+
+
+pytestmark = pytest.mark.unit
+
+
+def test_packet_header_pack_size_and_endianness():
+    header = st.sPacketHeader(
+        iMessage=int(st.MessageId.NAT_FRAMEOFDATA),
+        nDataBytes=42,
+    )
+    packed = header.pack()
+
+    assert len(packed) == ctypes.sizeof(st.sPacketHeader) == 4
+    message_id, payload_len = struct.unpack("<HH", packed)
+    assert message_id == int(st.MessageId.NAT_FRAMEOFDATA)
+    assert payload_len == 42
+
+
+def test_empty_frame_pack_length_is_stable():
+    frame = dt.sFrameOfMocapData()
+    frame.iFrame = 0
+
+    packed = frame.pack()
+
+    # 9 count fields + timestamps + params; all collections empty.
+    assert len(packed) == 86
+    assert packed == frame.pack()
+
+
+def test_single_rigid_body_frame_contains_id_and_params():
+    frame = dt.sFrameOfMocapData()
+    frame.iFrame = 7
+    frame.nRigidBodies = 1
+    rb = frame.RigidBodies[0]
+    rb.ID = 1
+    rb.x, rb.y, rb.z = 1.0, 2.0, 3.0
+    rb.qx, rb.qy, rb.qz, rb.qw = 0.0, 0.0, 0.0, 1.0
+    rb.params = 0x01
+
+    packed = frame.pack()
+
+    assert len(packed) == 86 + 38
+    body_offset = 4 + 4 + 4 + 4  # iFrame, nMarkerSets, nOtherMarkers, nRigidBodies
+    body_id, = struct.unpack_from("<i", packed, body_offset)
+    params_offset = body_offset + 4 + 8 * 4  # id + 8 floats (incl. MeanError)
+    params, = struct.unpack_from("<h", packed, params_offset)
+    assert body_id == 1
+    assert params == 0x01
+
+
+def test_server_description_pack_includes_connection_fields():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=1510,
+        data_port=1511,
+    )
+    description = server.server_description
+    packed = description.pack()
+
+    assert len(packed) == ctypes.sizeof(st.sServerDescription)
+    assert description.HostPresent is True
+    assert description.bConnectionInfoValid is True
+    assert description.ConnectionDataPort == 1511
+    assert description.ConnectionMulticast is False
+    assert description.NatNetVersion[0] == 4
+    assert description.NatNetVersion[1] == 4
+    assert description.szHostApp.startswith(b"Motive")
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
new file mode 100644
index 000000000..cca022851
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
@@ -0,0 +1,113 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""UDP loopback protocol tests for NatNetUnicastServer."""
+
+from __future__ import annotations
+
+import ctypes
+import socket
+import struct
+import sys
+import time
+from pathlib import Path
+
+import pytest
+
+_TEST_DIR = Path(__file__).resolve().parent
+if str(_TEST_DIR) not in sys.path:
+    sys.path.insert(0, str(_TEST_DIR))
+
+from natnet_test_helpers import NatNetTestClient, running_unicast_server
+
+from optitrack.natnet.emulator.server import natnet_data_types as dt
+from optitrack.natnet.emulator.server import natnet_server_types as st
+
+
+pytestmark = pytest.mark.unit
+
+
+def test_nat_connect_receives_serverinfo_on_command_port():
+    with running_unicast_server() as (server, command_port):
+        client = NatNetTestClient()
+        try:
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            message_id, payload, addr = client.recv_message()
+        finally:
+            client.close()
+
+        assert addr[1] == command_port
+        assert message_id == int(st.MessageId.NAT_SERVERINFO)
+        assert len(payload) == ctypes.sizeof(st.sServerDescription)
+
+        description = st.sServerDescription.from_buffer_copy(payload)
+        assert description.HostPresent is True
+        assert description.ConnectionDataPort == server.data_port
+
+
+def test_enqueued_frame_arrives_on_command_port():
+    with running_unicast_server(publish_rate=200) as (_server, command_port):
+        client = NatNetTestClient(timeout=3.0)
+        try:
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            client.recv_message()
+
+            frame = dt.sFrameOfMocapData()
+            frame.iFrame = 42
+            _server.enqueue_mocap_data(frame)
+
+            deadline = time.time() + 2.0
+            got_frame = False
+            while time.time() < deadline:
+                try:
+                    message_id, _payload, addr = client.recv_message()
+                except socket.timeout:
+                    break
+                if message_id == int(st.MessageId.NAT_FRAMEOFDATA):
+                    assert addr[1] == command_port
+                    got_frame = True
+                    break
+        finally:
+            client.close()
+
+        assert got_frame, "Expected NAT_FRAMEOFDATA on the command socket"
+
+
+def test_no_frame_sent_without_connected_client():
+    with running_unicast_server(publish_rate=200) as (server, command_port):
+        listener = NatNetTestClient(timeout=0.5)
+        try:
+            frame = dt.sFrameOfMocapData()
+            frame.iFrame = 1
+            server.enqueue_mocap_data(frame)
+            time.sleep(0.05)
+
+            with pytest.raises(socket.timeout):
+                listener.recv_message()
+        finally:
+            listener.close()
+
+        assert command_port  # server bound; no clients registered
+
+
+def test_connect_registers_client_for_subsequent_frames():
+    with running_unicast_server(publish_rate=200) as (server, command_port):
+        client_a = NatNetTestClient(timeout=3.0)
+        client_b = NatNetTestClient(timeout=0.5)
+        try:
+            client_a.send_message(command_port, st.MessageId.NAT_CONNECT)
+            client_a.recv_message()
+
+            frame = dt.sFrameOfMocapData()
+            frame.iFrame = 99
+            server.enqueue_mocap_data(frame)
+
+            message_id, payload, addr = client_a.recv_message()
+            assert message_id == int(st.MessageId.NAT_FRAMEOFDATA)
+            frame_num, = struct.unpack_from("<i", payload, 0)
+            assert frame_num == 99
+
+            with pytest.raises(socket.timeout):
+                client_b.recv_message()
+        finally:
+            client_a.close()
+            client_b.close()
diff --git a/tests/conftest.py b/tests/conftest.py
index 31fd29076..d74742dc7 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -124,13 +124,25 @@ def pytest_addoption(parser):
                           "sweep in test_takeoff_hover_land. Default: 0.5,1,2")
 
 
+def _chmod_world_writable(path: Path) -> None:
+    """Best-effort: allow Docker and host users to share tests/results."""
+    try:
+        path.chmod(0o777)
+    except OSError:
+        pass
+
+
 def pytest_configure(config):
     global RUN_DIR, LOGS_DIR
     timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
     results_root = Path(AIRSTACK_ROOT) / "tests" / "results"
+    results_root.mkdir(parents=True, exist_ok=True)
+    _chmod_world_writable(results_root)
     RUN_DIR = results_root / timestamp
     LOGS_DIR = RUN_DIR / "logs"
     LOGS_DIR.mkdir(parents=True, exist_ok=True)
+    _chmod_world_writable(RUN_DIR)
+    _chmod_world_writable(LOGS_DIR)
     config.option.xmlpath = str(RUN_DIR / "results.xml")
 
 
diff --git a/tests/sim/optitrack_natnet_emulator/test_serializers.py b/tests/sim/optitrack_natnet_emulator/test_serializers.py
new file mode 100644
index 000000000..58f7f3099
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_serializers.py
@@ -0,0 +1,20 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator unit tests for pytest tests/."""
+
+import importlib.util
+from pathlib import Path
+
+import pytest
+
+_repo_root = Path(__file__).resolve().parents[3]
+_pkg_test = _repo_root / "simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"
+_real_file = _pkg_test / "test_serializers.py"
+
+_spec = importlib.util.spec_from_file_location("_optitrack_natnet_serializers", _real_file)
+_real = importlib.util.module_from_spec(_spec)
+_spec.loader.exec_module(_real)
+
+for _name in dir(_real):
+    if _name.startswith("test_"):
+        globals()[_name] = pytest.mark.unit(getattr(_real, _name))
diff --git a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
new file mode 100644
index 000000000..0840b4f78
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
@@ -0,0 +1,20 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator unit tests for pytest tests/."""
+
+import importlib.util
+from pathlib import Path
+
+import pytest
+
+_repo_root = Path(__file__).resolve().parents[3]
+_pkg_test = _repo_root / "simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"
+_real_file = _pkg_test / "test_unicast_protocol.py"
+
+_spec = importlib.util.spec_from_file_location("_optitrack_natnet_unicast_protocol", _real_file)
+_real = importlib.util.module_from_spec(_spec)
+_spec.loader.exec_module(_real)
+
+for _name in dir(_real):
+    if _name.startswith("test_"):
+        globals()[_name] = pytest.mark.unit(getattr(_real, _name))

From 11522bb9d0643a74f7363f8d1e999b4304e972ca Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Fri, 5 Jun 2026 16:01:19 -0400
Subject: [PATCH 06/27] model definition implementation with unit tests

---
 .../natnet/emulator/server/natnet_common.py   |  27 ++
 .../emulator/server/natnet_data_types.py      |  24 +-
 .../emulator/server/natnet_model_types.py     | 121 +++++++++
 .../emulator/server/natnet_server_types.py    |  10 -
 .../emulator/server/natnet_unicast_server.py  |  29 ++-
 .../test/natnet_test_helpers.py               |  25 +-
 .../test/test_serializers.py                  | 239 ++++++++++++++++++
 .../test/test_unicast_protocol.py             | 132 ++++++++++
 8 files changed, 568 insertions(+), 39 deletions(-)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py
new file mode 100644
index 000000000..1eb32177c
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py
@@ -0,0 +1,27 @@
+from enum import IntEnum
+import ctypes
+
+class ModelLimits(IntEnum):
+    MAX_MODELS = 2000    # maximum number of total models (data descriptions)
+    MAX_MARKERSETS = 1000    # maximum number of MarkerSets 
+    MAX_RIGIDBODIES = 1000    # maximum number of RigidBodies
+    MAX_ASSETS = 1000    # Maximum number of Assets
+    MAX_NAMELENGTH = 256     # maximum length for strings
+    MAX_MARKERS = 200     # maximum number of markers per MarkerSet
+    MAX_RBMARKERS = 20      # maximum number of markers per RigidBody
+    MAX_SKELETONS = 100     # maximum number of skeletons
+    MAX_SKELRIGIDBODIES = 200     # maximum number of RididBodies per Skeleton
+    MAX_LABELED_MARKERS = 1000    # maximum number of labeled markers per frame
+    MAX_UNLABELED_MARKERS = 1000    # maximum number of unlabeled (other) markers per frame
+
+    MAX_FORCEPLATES = 100     # maximum number of force plate 'bundles'
+    MAX_DEVICES = 100     # maximum number of peripheral device 'bundles'
+    MAX_ANALOG_CHANNELS = 32      # maximum number of data channels (signals) per analog/force plate device
+    MAX_ANALOG_SUBFRAMES = 30      # maximum number of analog/force plate frames per mocap frame
+
+    MAX_PACKETSIZE = 65503   # max size of packet in bytes (actual packet size is dynamic)
+                                                # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes)
+
+
+
+MarkerData = ctypes.c_float * 3
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
index f9d41ec5d..2f48db397 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
@@ -1,27 +1,6 @@
 import ctypes
 import struct
-from enum import Enum, IntEnum
-
-class ModelLimits(IntEnum):
-    MAX_MODELS = 2000    # maximum number of total models (data descriptions)
-    MAX_MARKERSETS = 1000    # maximum number of MarkerSets 
-    MAX_RIGIDBODIES = 1000    # maximum number of RigidBodies
-    MAX_ASSETS = 1000    # Maximum number of Assets
-    MAX_NAMELENGTH = 256     # maximum length for strings
-    MAX_MARKERS = 200     # maximum number of markers per MarkerSet
-    MAX_RBMARKERS = 20      # maximum number of markers per RigidBody
-    MAX_SKELETONS = 100     # maximum number of skeletons
-    MAX_SKELRIGIDBODIES = 200     # maximum number of RididBodies per Skeleton
-    MAX_LABELED_MARKERS = 1000    # maximum number of labeled markers per frame
-    MAX_UNLABELED_MARKERS = 1000    # maximum number of unlabeled (other) markers per frame
-
-    MAX_FORCEPLATES = 100     # maximum number of force plate 'bundles'
-    MAX_DEVICES = 100     # maximum number of peripheral device 'bundles'
-    MAX_ANALOG_CHANNELS = 32      # maximum number of data channels (signals) per analog/force plate device
-    MAX_ANALOG_SUBFRAMES = 30      # maximum number of analog/force plate frames per mocap frame
-
-    MAX_PACKETSIZE = 65503   # max size of packet in bytes (actual packet size is dynamic)
-                                                # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes)
+from .natnet_common import ModelLimits, MarkerData
 
 class sMarker(ctypes.Structure):
     _pack_ = 1
@@ -38,7 +17,6 @@ class sMarker(ctypes.Structure):
     def pack(self) -> bytes:
         return struct.pack('<i5fhf', self.ID, self.x, self.y, self.z, self.size, self.params, self.residual)
 
-MarkerData = ctypes.c_float * 3
 
 class sMarkerSetData(ctypes.Structure):
     _pack_ = 1
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
new file mode 100644
index 000000000..049d05dff
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
@@ -0,0 +1,121 @@
+import ctypes
+import struct
+from enum import IntEnum
+
+from .natnet_common import MarkerData, ModelLimits
+
+
+# NatNet data types
+class DataDescriptors(IntEnum):
+    Descriptor_MarkerSet = 0
+    Descriptor_RigidBody = 1
+    Descriptor_Skeleton = 2
+    Descriptor_ForcePlate = 3
+    Descriptor_Device = 4
+    Descriptor_Camera = 5
+    Descriptor_Asset = 6
+
+
+# Rigid Body Definition
+class sRigidBodyDescription(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("szName", ctypes.c_char * ModelLimits.MAX_NAMELENGTH),  # RigidBody name
+        ("ID", ctypes.c_int32),  # RigidBody identifier: Streaming ID value for rigid body assets, and Bone index value for skeleton rigid bodies.
+        ("parentID", ctypes.c_int32),  # ID of parent Rigid Body (in case hierarchy exists; otherwise -1)
+        ("offsetx", ctypes.c_float),  # offset position relative to parent
+        ("offsety", ctypes.c_float),  # offset position relative to parent
+        ("offsetz", ctypes.c_float),  # offset position relative to parent
+        ("offsetqx", ctypes.c_float),  # Quaternion rotational offset relative to parent for skeleton bones
+        ("offsetqy", ctypes.c_float),  # Quaternion rotational offset relative to parent for skeleton bones
+        ("offsetqz", ctypes.c_float),  # Quaternion rotational offset relative to parent for skeleton bones
+        ("offsetqw", ctypes.c_float),  # Quaternion rotational offset relative to parent for skeleton bones
+        ("nMarkers", ctypes.c_int32),  # Number of markers associated with this rigid body
+        ("MarkerPositions", MarkerData * ModelLimits.MAX_RBMARKERS),  # Array of marker locations ( [nMarkers][3] )
+        ("MarkerRequiredLabels", ctypes.c_int32 * ModelLimits.MAX_RBMARKERS),  # Array of expected marker active labels - 0 if not specified. ( [nMarkers] )
+        ("szMarkerNames", ctypes.c_char * ModelLimits.MAX_NAMELENGTH * ModelLimits.MAX_RBMARKERS),  # Array of marker names  ( [nMarkers][MAX_NAMELENGTH] )
+    ]
+
+    def pack(self) -> bytes:
+        # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH.
+        name_bytes = self.szName.rstrip(b"\x00") + b"\x00"
+        payload = bytearray(name_bytes)
+        payload += struct.pack(
+            "<ii",
+            self.ID,
+            self.parentID,
+        )
+        payload += struct.pack(
+            "<3f",
+            self.offsetx,
+            self.offsety,
+            self.offsetz,
+        )
+        payload += struct.pack(
+            "<4f",
+            self.offsetqx,
+            self.offsetqy,
+            self.offsetqz,
+            self.offsetqw,
+        )
+        payload += struct.pack("<i", self.nMarkers)
+        for i in range(self.nMarkers):
+            payload += struct.pack(
+                "<3f",
+                self.MarkerPositions[i][0],
+                self.MarkerPositions[i][1],
+                self.MarkerPositions[i][2],
+            )
+        for i in range(self.nMarkers):
+            payload += struct.pack("<i", self.MarkerRequiredLabels[i])
+        for i in range(self.nMarkers):
+            name_offset = i * ModelLimits.MAX_NAMELENGTH
+            marker_name = self.szMarkerNames[
+                name_offset : name_offset + ModelLimits.MAX_NAMELENGTH
+            ]
+            payload += marker_name.rstrip(b"\x00") + b"\x00"
+        return bytes(payload)
+
+
+# Tracked Object data description.
+# A Mocap Server application (e.g. Arena or TrackingTools) may contain multiple
+# tracked "objects (e.g. RigidBody, MarkerSet).  Each object will have its
+# own DataDescription.
+class sDataDescription(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("type", ctypes.c_int32),
+        # union Data (NatNet SDK):
+        #   sMarkerSetDescription*  MarkerSetDescription;
+        #   sRigidBodyDescription*  RigidBodyDescription;
+        #   sSkeletonDescription*   SkeletonDescription;
+        #   sForcePlateDescription* ForcePlateDescription;
+        #   sDeviceDescription*     DeviceDescription;
+        #   sCameraDescription*     CameraDescription;
+        #   sAssetDescription*      AssetDescription;
+        ("RigidBodyDescription", sRigidBodyDescription),
+    ]
+
+    def pack(self) -> bytes:
+        if self.type == int(DataDescriptors.Descriptor_RigidBody):
+            body = self.RigidBodyDescription.pack()
+        else:
+            raise ValueError(f"Unsupported data description type: {self.type}")
+        payload = bytearray(struct.pack("<ii", self.type, len(body)))
+        payload += body
+        return bytes(payload)
+
+
+# All data descriptions for current session (as defined by host app)
+class sDataDescriptions(ctypes.Structure):
+    _pack_ = 1
+    _fields_ = [
+        ("nDataDescriptions", ctypes.c_int32),
+        ("arrDataDescriptions", sDataDescription * ModelLimits.MAX_MODELS),
+    ]
+
+    def pack(self) -> bytes:
+        payload = bytearray(struct.pack("<i", self.nDataDescriptions))
+        for i in range(self.nDataDescriptions):
+            payload += self.arrDataDescriptions[i].pack()
+        return bytes(payload)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
index ad9945a5e..d2c1433a9 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
@@ -23,16 +23,6 @@ class MessageId(IntEnum):
     NAT_DISCOVERY               = 14
     NAT_UNRECOGNIZED_REQUEST    = 100
 
-# NatNet data types / Descriptors
-class DataDescriptors(IntEnum):
-    Descriptor_MarkerSet = 0
-    Descriptor_RigidBody = 1
-    Descriptor_Skeleton = 2
-    Descriptor_ForcePlate = 3
-    Descriptor_Device = 4
-    Descriptor_Camera = 5
-    Descriptor_Asset = 6
-
 # Server/Sender configuration and info
 class sSender(ctypes.Structure):
     _pack_ = 1
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index 5ed7b3a76..fb431e87f 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -102,6 +102,31 @@ def _handle_command_request(self, request_data: bytes, client_address: tuple): #
                 raise ValueError(
                     f"[Command Handler] Error sending server description to client {client_address}: {e}"
                 ) from e
-            print(f"[Command Handler] Sent server description to client address through its port {client_address}.")
-        
+            print(
+                f"[Command Handler] Sent server description to client address "
+                f"through its port {client_address}."
+            )
+            return
+
+        # Non-handshake commands require a prior NAT_CONNECT from this endpoint.
+        client_ip, client_port = client_address
+        client = self._find_client(client_ip, client_port)
+        if client is None:
+            print(
+                f"[Command Handler] Ignoring message {header.iMessage} from "
+                f"unregistered client {client_address}."
+            )
+            return
 
+        print(
+            f"[Command Handler] Unhandled message id {header.iMessage} from "
+            f"registered client {client_address}."
+        )
+
+    def _find_client(self, ip: str, port: int) -> Client | None:
+        target = Client(ip, port)
+        with self.clients_lock:
+            for client in self.connected_clients:
+                if client == target:
+                    return client
+        return None
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
index d7e12413a..3e601f01c 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
@@ -48,10 +48,27 @@ def send_message(
             iMessage=int(message_id),
             nDataBytes=len(payload),
         )
-        self._sock.sendto(
-            header.pack() + payload,
-            (server_host or self._host, server_port),
-        )
+        self.send_raw(header.pack() + payload, server_port, server_host)
+
+    def send_raw(
+        self,
+        data: bytes,
+        server_port: int,
+        server_host: str | None = None,
+    ) -> None:
+        """Send a raw UDP datagram (for malformed / malicious packet tests)."""
+        self._sock.sendto(data, (server_host or self._host, server_port))
+
+    def send_header_only(
+        self,
+        server_port: int,
+        message_id: st.MessageId | int,
+        declared_payload_len: int,
+        server_host: str | None = None,
+    ) -> None:
+        """Send a header whose nDataBytes does not match any trailing payload."""
+        header = struct.pack("<HH", int(message_id), declared_payload_len)
+        self.send_raw(header, server_port, server_host)
 
     def recv_message(self) -> tuple[int, bytes, tuple[str, int]]:
         data, addr = self._sock.recvfrom(65535)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
index 9ae58b093..f2c9a8786 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
@@ -17,12 +17,19 @@
 
 from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
 from optitrack.natnet.emulator.server import natnet_data_types as dt
+from optitrack.natnet.emulator.server import natnet_model_types as mt
 from optitrack.natnet.emulator.server import natnet_server_types as st
+from optitrack.natnet.emulator.server.natnet_common import ModelLimits
 
 
 pytestmark = pytest.mark.unit
 
 
+# =============================================================================
+# natnet_server_types — transport / handshake
+# =============================================================================
+
+
 def test_packet_header_pack_size_and_endianness():
     header = st.sPacketHeader(
         iMessage=int(st.MessageId.NAT_FRAMEOFDATA),
@@ -36,6 +43,23 @@ def test_packet_header_pack_size_and_endianness():
     assert payload_len == 42
 
 
+def test_packet_header_length_exceeds_max_packetsize():
+    # Encoders can lie in nDataBytes; receivers must bound reads separately.
+    header = st.sPacketHeader(
+        iMessage=int(st.MessageId.NAT_FRAMEOFDATA),
+        nDataBytes=ModelLimits.MAX_PACKETSIZE + 1,
+    )
+    packed = header.pack()
+
+    _message_id, payload_len = struct.unpack("<HH", packed)
+    assert payload_len == ModelLimits.MAX_PACKETSIZE + 1
+
+
+# =============================================================================
+# natnet_data_types — frame streaming (NAT_FRAMEOFDATA payload)
+# =============================================================================
+
+
 def test_empty_frame_pack_length_is_stable():
     frame = dt.sFrameOfMocapData()
     frame.iFrame = 0
@@ -68,6 +92,30 @@ def test_single_rigid_body_frame_contains_id_and_params():
     assert params == 0x01
 
 
+def test_frame_pack_rejects_rigid_body_count_above_max():
+    frame = dt.sFrameOfMocapData()
+    frame.nRigidBodies = ModelLimits.MAX_RIGIDBODIES + 1
+
+    with pytest.raises(IndexError):
+        frame.pack()
+
+
+def test_frame_pack_negative_collection_counts_encode_without_validation():
+    # Encoder misuse: negative counts serialize to the wire without raising.
+    frame = dt.sFrameOfMocapData()
+    frame.nRigidBodies = -1
+
+    packed = frame.pack()
+
+    n_rigid_bodies, = struct.unpack_from("<i", packed, 12)
+    assert n_rigid_bodies == -1
+
+
+# =============================================================================
+# natnet_server_types — server description (NAT_SERVERINFO payload)
+# =============================================================================
+
+
 def test_server_description_pack_includes_connection_fields():
     server = NatNetUnicastServer(
         local_interface="127.0.0.1",
@@ -87,3 +135,194 @@ def test_server_description_pack_includes_connection_fields():
     assert description.NatNetVersion[0] == 4
     assert description.NatNetVersion[1] == 4
     assert description.szHostApp.startswith(b"Motive")
+
+
+def test_server_init_rejects_invalid_local_interface():
+    with pytest.raises(ValueError, match="Invalid local interface IP address"):
+        NatNetUnicastServer(local_interface="not-an-ip")
+
+
+def test_server_init_rejects_identical_command_and_data_ports():
+    with pytest.raises(ValueError, match="Command port and data port must be different"):
+        NatNetUnicastServer(
+            local_interface="127.0.0.1",
+            transmission_type=TransmissionType.UNICAST,
+            command_port=1510,
+            data_port=1510,
+        )
+
+
+# =============================================================================
+# natnet_model_types — rigid body description (MODELDEF entry body)
+# =============================================================================
+
+
+def test_rigid_body_description_pack_zero_markers():
+    rb = mt.sRigidBodyDescription()
+    rb.szName = b"Drone"
+    rb.ID = 1
+    rb.parentID = -1
+    rb.offsetqw = 1.0
+    rb.nMarkers = 0
+
+    packed = rb.pack()
+
+    assert packed.startswith(b"Drone\x00")
+    body_id, parent_id = struct.unpack_from("<ii", packed, len(b"Drone\x00"))
+    assert body_id == 1
+    assert parent_id == -1
+    pos = struct.unpack_from("<3f", packed, len(b"Drone\x00") + 8)
+    rot = struct.unpack_from("<4f", packed, len(b"Drone\x00") + 8 + 12)
+    n_markers, = struct.unpack_from("<i", packed, len(b"Drone\x00") + 8 + 12 + 16)
+    assert pos == (0.0, 0.0, 0.0)
+    assert rot == (0.0, 0.0, 0.0, 1.0)
+    assert n_markers == 0
+
+
+def test_rigid_body_description_pack_rejects_marker_count_above_max():
+    rb = mt.sRigidBodyDescription()
+    rb.szName = b"Drone"
+    rb.nMarkers = ModelLimits.MAX_RBMARKERS + 1
+
+    with pytest.raises(IndexError):
+        rb.pack()
+
+
+def test_rigid_body_description_pack_negative_marker_count_encodes_without_validation():
+    rb = mt.sRigidBodyDescription()
+    rb.szName = b"Drone"
+    rb.nMarkers = -1
+
+    packed = rb.pack()
+
+    n_markers, = struct.unpack_from("<i", packed, len(b"Drone\x00") + 8 + 12 + 16)
+    assert n_markers == -1
+
+
+def test_rigid_body_description_pack_empty_name():
+    rb = mt.sRigidBodyDescription()
+    rb.nMarkers = 0
+
+    packed = rb.pack()
+
+    assert packed.startswith(b"\x00")
+
+
+# =============================================================================
+# natnet_model_types — data descriptions (NAT_MODELDEF payload)
+# =============================================================================
+
+
+def test_data_descriptions_pack_single_rigid_body():
+    descriptions = mt.sDataDescriptions()
+    descriptions.nDataDescriptions = 1
+    desc = descriptions.arrDataDescriptions[0]
+    desc.type = int(mt.DataDescriptors.Descriptor_RigidBody)
+    desc.RigidBodyDescription.szName = b"Drone"
+    desc.RigidBodyDescription.ID = 1
+    desc.RigidBodyDescription.parentID = -1
+    desc.RigidBodyDescription.offsetqw = 1.0
+    desc.RigidBodyDescription.nMarkers = 0
+
+    payload = descriptions.pack()
+
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    desc_type, size_bytes = struct.unpack_from("<ii", payload, 4)
+    assert n_descriptions == 1
+    assert desc_type == int(mt.DataDescriptors.Descriptor_RigidBody)
+    body = payload[12 : 12 + size_bytes]
+    assert body.startswith(b"Drone\x00")
+    assert len(body) == size_bytes
+
+
+def test_data_descriptions_pack_empty():
+    descriptions = mt.sDataDescriptions()
+    descriptions.nDataDescriptions = 0
+
+    payload = descriptions.pack()
+
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    assert n_descriptions == 0
+    assert len(payload) == 4
+
+
+def test_data_descriptions_pack_multiple_rigid_bodies():
+    descriptions = mt.sDataDescriptions()
+    descriptions.nDataDescriptions = 3
+
+    bodies = [
+        ("Drone", 1, -1),
+        ("Target", 2, -1),
+        ("Payload", 3, 1),
+    ]
+    for index, (name, body_id, parent_id) in enumerate(bodies):
+        desc = descriptions.arrDataDescriptions[index]
+        desc.type = int(mt.DataDescriptors.Descriptor_RigidBody)
+        desc.RigidBodyDescription.szName = name.encode("utf-8")
+        desc.RigidBodyDescription.ID = body_id
+        desc.RigidBodyDescription.parentID = parent_id
+        desc.RigidBodyDescription.offsetqw = 1.0
+        desc.RigidBodyDescription.nMarkers = 0
+
+    payload = descriptions.pack()
+
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    assert n_descriptions == 3
+
+    offset = 4
+    parsed = []
+    for expected_name, expected_id, expected_parent_id in bodies:
+        desc_type, size_bytes = struct.unpack_from("<ii", payload, offset)
+        offset += 8
+        body = payload[offset : offset + size_bytes]
+        offset += size_bytes
+
+        assert desc_type == int(mt.DataDescriptors.Descriptor_RigidBody)
+        assert body.startswith(expected_name.encode("utf-8") + b"\x00")
+        name_len = len(expected_name) + 1
+        body_id, parent_id = struct.unpack_from("<ii", body, name_len)
+        parsed.append((body_id, parent_id))
+
+    assert parsed == [(1, -1), (2, -1), (3, 1)]
+    assert offset == len(payload)
+
+
+def test_data_description_pack_rejects_unsupported_type():
+    desc = mt.sDataDescription()
+    desc.type = 999
+
+    with pytest.raises(ValueError, match="Unsupported data description type"):
+        desc.pack()
+
+
+def test_data_description_pack_rejects_negative_type():
+    desc = mt.sDataDescription()
+    desc.type = -1
+
+    with pytest.raises(ValueError, match="Unsupported data description type"):
+        desc.pack()
+
+
+def test_data_descriptions_pack_rejects_count_above_max():
+    descriptions = mt.sDataDescriptions()
+    for i in range(ModelLimits.MAX_MODELS):
+        desc = descriptions.arrDataDescriptions[i]
+        desc.type = int(mt.DataDescriptors.Descriptor_RigidBody)
+        desc.RigidBodyDescription.szName = b"Body"
+        desc.RigidBodyDescription.ID = i + 1
+        desc.RigidBodyDescription.nMarkers = 0
+    descriptions.nDataDescriptions = ModelLimits.MAX_MODELS + 1
+
+    with pytest.raises(IndexError):
+        descriptions.pack()
+
+
+def test_data_descriptions_pack_negative_count_encodes_without_validation():
+    descriptions = mt.sDataDescriptions()
+    descriptions.nDataDescriptions = -1
+
+    payload = descriptions.pack()
+
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    assert n_descriptions == -1
+    assert len(payload) == 4
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
index cca022851..8d3ab10df 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
@@ -19,6 +19,7 @@
 
 from natnet_test_helpers import NatNetTestClient, running_unicast_server
 
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
 from optitrack.natnet.emulator.server import natnet_data_types as dt
 from optitrack.natnet.emulator.server import natnet_server_types as st
 
@@ -26,6 +27,11 @@
 pytestmark = pytest.mark.unit
 
 
+# =============================================================================
+# Handshake — NAT_CONNECT → NAT_SERVERINFO (command socket)
+# =============================================================================
+
+
 def test_nat_connect_receives_serverinfo_on_command_port():
     with running_unicast_server() as (server, command_port):
         client = NatNetTestClient()
@@ -44,6 +50,11 @@ def test_nat_connect_receives_serverinfo_on_command_port():
         assert description.ConnectionDataPort == server.data_port
 
 
+# =============================================================================
+# Frame streaming — NAT_FRAMEOFDATA on command socket (libNatNet 4.4 unicast)
+# =============================================================================
+
+
 def test_enqueued_frame_arrives_on_command_port():
     with running_unicast_server(publish_rate=200) as (_server, command_port):
         client = NatNetTestClient(timeout=3.0)
@@ -72,6 +83,11 @@ def test_enqueued_frame_arrives_on_command_port():
         assert got_frame, "Expected NAT_FRAMEOFDATA on the command socket"
 
 
+# =============================================================================
+# Client registration — connected vs unconnected endpoints
+# =============================================================================
+
+
 def test_no_frame_sent_without_connected_client():
     with running_unicast_server(publish_rate=200) as (server, command_port):
         listener = NatNetTestClient(timeout=0.5)
@@ -111,3 +127,119 @@ def test_connect_registers_client_for_subsequent_frames():
         finally:
             client_a.close()
             client_b.close()
+
+
+# =============================================================================
+# Malformed datagrams — handler-level (no network)
+# =============================================================================
+
+
+def test_truncated_datagram_is_ignored():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        command_port=1510,
+        data_port=1511,
+    )
+    server._handle_command_request(b"\x00\x01", ("127.0.0.1", 40000))
+    assert len(server.connected_clients) == 0
+
+
+def test_empty_datagram_is_ignored():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        command_port=1510,
+        data_port=1511,
+    )
+    server._handle_command_request(b"", ("127.0.0.1", 40000))
+    assert len(server.connected_clients) == 0
+
+
+# =============================================================================
+# Malformed datagrams — unregistered client (no reply)
+# =============================================================================
+
+
+def test_unknown_message_id_from_unregistered_client_gets_no_reply():
+    with running_unicast_server() as (_server, command_port):
+        client = NatNetTestClient(timeout=0.5)
+        try:
+            client.send_message(command_port, 999)
+            with pytest.raises(socket.timeout):
+                client.recv_message()
+        finally:
+            client.close()
+
+
+def test_request_modeldef_without_connect_gets_no_reply():
+    with running_unicast_server() as (_server, command_port):
+        client = NatNetTestClient(timeout=0.5)
+        try:
+            client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF)
+            with pytest.raises(socket.timeout):
+                client.recv_message()
+        finally:
+            client.close()
+
+
+# =============================================================================
+# Malformed datagrams — handshake edge cases
+# =============================================================================
+
+
+def test_lie_about_payload_length_in_header_still_handshake_on_connect():
+    with running_unicast_server() as (_server, command_port):
+        client = NatNetTestClient(timeout=2.0)
+        try:
+            client.send_header_only(
+                command_port,
+                st.MessageId.NAT_CONNECT,
+                declared_payload_len=65535,
+            )
+            message_id, payload, addr = client.recv_message()
+        finally:
+            client.close()
+
+        assert addr[1] == command_port
+        assert message_id == int(st.MessageId.NAT_SERVERINFO)
+        assert len(payload) == ctypes.sizeof(st.sServerDescription)
+
+
+# =============================================================================
+# Malformed datagrams — registered client & recovery
+# =============================================================================
+
+
+def test_unknown_message_from_registered_client_gets_no_reply():
+    with running_unicast_server() as (server, command_port):
+        client = NatNetTestClient(timeout=0.5)
+        try:
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            client.recv_message()
+
+            client.send_message(command_port, 999)
+            with pytest.raises(socket.timeout):
+                client.recv_message()
+
+            assert len(server.connected_clients) == 1
+        finally:
+            client.close()
+
+
+def test_server_survives_malformed_burst_then_valid_connect():
+    with running_unicast_server() as (server, command_port):
+        client = NatNetTestClient(timeout=2.0)
+        try:
+            client.send_raw(b"", command_port)
+            client.send_raw(b"\xff", command_port)
+            client.send_header_only(command_port, 999, declared_payload_len=50000)
+            client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF)
+
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            message_id, _payload, _addr = client.recv_message()
+        finally:
+            client.close()
+
+        assert message_id == int(st.MessageId.NAT_SERVERINFO)
+        assert len(server.connected_clients) == 1

From 5248ff124d7b8415f2be13e6ed3876c13d98bee9 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Mon, 8 Jun 2026 12:39:53 -0400
Subject: [PATCH 07/27] feat(natnet-emulator): MODELDEF payload cache,
 defaults, and Phase 5 handlers

Refactor the emulator server around a pre-packed MODELDEF wire cache and add
the NAT_REQUEST_MODELDEF and NAT_KEEPALIVE handlers so libNatNet 4.4 clients
complete the unicast handshake. Mock a real Motive server (server name
"Motive") and move hardcoded Drone reference constants into a new defaults.py
so scene semantics stay out of the wire layer. Drop the vendored NatNet SDK
README that should not live in-tree.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .../NatNetClientSDK/README.md                 |   9 --
 .../optitrack.natnet.emulator/README.md       |  27 ++++-
 .../optitrack/natnet/emulator/__init__.py     |  10 ++
 .../optitrack/natnet/emulator/defaults.py     |  28 +++++
 .../emulator/server/natnet_data_types.py      |  78 ++++++++++----
 .../emulator/server/natnet_model_types.py     |  15 +++
 .../natnet/emulator/server/natnet_server.py   | 102 ++++++++++++------
 .../emulator/server/natnet_server_types.py    |  54 +++++++++-
 .../emulator/server/natnet_unicast_server.py  |  58 +++++++---
 9 files changed, 301 insertions(+), 80 deletions(-)
 delete mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
deleted file mode 100644
index 38d3112cb..000000000
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/README.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# NatNetClientSDK (reference only)
-
-Local copy of the OptiTrack NatNet 4.4 SDK samples and headers for **protocol development**.
-
-- **Not** installed by the Isaac extension wheel
-- **Not** redistributed by AirStack (OptiTrack license applies)
-- Use for wire-format reference: `NatNetSDK/include/NatNetTypes.h`, `Samples/PacketClient/PacketClient.cpp`
-
-Production robot client uses `libNatNet.so` via `airstack setup --natnet` in [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/).
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
index aff90f02b..17d99f52d 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
@@ -9,10 +9,12 @@ optitrack.natnet.emulator/
 ├── config/extension.toml       # Isaac Sim extension manifest (stub)
 ├── setup.py
 ├── optitrack/natnet/emulator/
+│   ├── defaults.py             # Test / Isaac reference constants (not used by server)
 │   └── server/                 # NatNet UDP server (transport + protocol)
 │       ├── natnet_server.py
 │       ├── natnet_unicast_server.py
 │       ├── natnet_data_types.py
+│       ├── natnet_model_types.py
 │       └── natnet_server_types.py
 └── NatNetClientSDK/            # Reference SDK only (not shipped in wheel)
 ```
@@ -21,15 +23,20 @@ optitrack.natnet.emulator/
 
 | Layer | Role |
 |-------|------|
-| **Server** (`optitrack.natnet.emulator.server`) | UDP transport, `NAT_CONNECT` / `NAT_SERVERINFO`, frame relay via `enqueue_mocap_data()` |
-| **Isaac wrapper** (planned) | Build `sFrameOfMocapData` from sim poses; register rigid-body model catalog |
+| **Server** (`optitrack.natnet.emulator.server`) | UDP transport, `NAT_CONNECT` / `NAT_SERVERINFO`, MODELDEF **wire cache**, frame relay via `enqueue_mocap_data()` |
+| **Isaac wrapper** (planned) | Build catalog from scene config, `set_model_def_payload(catalog.pack())`, sample prims → `enqueue_mocap_data()` |
+| **`defaults.py`** | Hardcoded Drone → `/World/base_link` binding for tests and future Isaac wrapper; **not imported by the server** |
+
+The server stores MODELDEF as packed **bytes** (`_model_def_payload`). It does not own prim-path bindings or ctypes catalog copies. The Isaac wrapper (or integration tests) call `set_model_def_payload()` after building `sDataDescriptions`.
 
 ## Usage (development)
 
 ```python
-from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType, make_default_drone_catalog
 
 server = NatNetUnicastServer(local_interface="172.31.0.200")
+# Default Drone MODELDEF is loaded on init; override from Isaac wrapper:
+# server.set_model_def_payload(make_default_drone_catalog().pack())
 server.start()
 # Wrapper enqueues pre-built frames:
 # server.enqueue_mocap_data(frame)
@@ -39,7 +46,19 @@ Default Docker sim IP: `172.31.0.200` (Isaac container on AirStack bridge networ
 
 ## Protocol notes
 
-For libNatNet 4.4 **unicast**, the client uses a single UDP socket; server replies and frames go to the client's `NAT_CONNECT` source endpoint on the **command port** (1510). See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md).
+For libNatNet 4.4 **unicast**, the client uses a single UDP socket; server replies and frames go to the client's `NAT_CONNECT` source endpoint on the **command port** (1510). Handlers: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, `NAT_KEEPALIVE`. See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md).
+
+## Tests
+
+| Tier | Mark | Location |
+|------|------|----------|
+| Unit (serializers, protocol, payload cache) | `unit` | `test/` + proxy in `tests/sim/optitrack_natnet_emulator/` |
+| Integration (natnet_ros2 loopback Hz) | `integration`, `natnet` | `tests/integration/natnet/test_natnet_integration.py` |
+
+```bash
+pytest tests/sim/optitrack_natnet_emulator/ -m unit -v
+pytest tests/integration/natnet/ -m natnet -v   # requires robot container + NatNet SDK
+```
 
 ## Reference material
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
index 20e405b79..e819d8b1b 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py
@@ -1,10 +1,20 @@
 """OptiTrack Motive NatNet emulator for Isaac Sim."""
 
+from .defaults import (
+    DEFAULT_DRONE_BINDING,
+    DEFAULT_TRACKED_BODY_BINDINGS,
+    TrackedBodyBinding,
+)
 from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType
+from .server.natnet_model_types import make_default_drone_catalog
 
 __all__ = [
     "Client",
+    "DEFAULT_DRONE_BINDING",
+    "DEFAULT_TRACKED_BODY_BINDINGS",
     "NatNetServer",
     "NatNetUnicastServer",
+    "TrackedBodyBinding",
     "TransmissionType",
+    "make_default_drone_catalog",
 ]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
new file mode 100644
index 000000000..2a0afd935
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
@@ -0,0 +1,28 @@
+"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.
+
+Not imported by NatNetServer — the server stores MODELDEF as packed bytes only.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+
+@dataclass(frozen=True)
+class TrackedBodyBinding:
+    """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire)."""
+
+    name: str
+    id: int
+    prim_path: str
+    parent_id: int = -1
+
+
+# Single-drone Pegasus scenes (example_one_px4_pegasus_launch_script.py).
+DEFAULT_DRONE_BINDING = TrackedBodyBinding(
+    name="Drone",
+    id=1,
+    prim_path="/World/base_link",
+)
+
+DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
index 2f48db397..53e342bd5 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
@@ -176,42 +176,77 @@ class sFrameOfMocapData(ctypes.Structure):
         ("params", ctypes.c_int16)
     ]
 
-    def pack(self) -> bytes:
+    @staticmethod
+    def _pack_counted_section(count: int, data: bytes, *, natnet_major: int, natnet_minor: int) -> bytes:
+        """NatNet 4.1+ prefixes each collection with a 4-byte byte count (PacketClient UnpackDataSize)."""
+        payload = bytearray(struct.pack('<i', count))
+        if (natnet_major == 4 and natnet_minor > 0) or natnet_major > 4:
+            payload += struct.pack('<i', len(data))
+        payload += data
+        return bytes(payload)
+
+    def pack(self, natnet_major: int = 4, natnet_minor: int = 4) -> bytes:
         payload = bytearray()
-        
+
         payload += struct.pack('<i', self.iFrame)
 
-        payload += struct.pack('<i', self.nMarkerSets)
+        markerset_data = bytearray()
         for i in range(self.nMarkerSets):
-            payload += self.MocapData[i].pack()
+            markerset_data += self.MocapData[i].pack()
+        payload += self._pack_counted_section(
+            self.nMarkerSets, bytes(markerset_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nOtherMarkers)
+        other_marker_data = bytearray()
         for i in range(self.nOtherMarkers):
-            payload += struct.pack('<3f', self.OtherMarkers[i][0], self.OtherMarkers[i][1], self.OtherMarkers[i][2])
+            other_marker_data += struct.pack(
+                '<3f', self.OtherMarkers[i][0], self.OtherMarkers[i][1], self.OtherMarkers[i][2]
+            )
+        payload += self._pack_counted_section(
+            self.nOtherMarkers, bytes(other_marker_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nRigidBodies)
+        rigid_body_data = bytearray()
         for i in range(self.nRigidBodies):
-            payload += self.RigidBodies[i].pack()
+            rigid_body_data += self.RigidBodies[i].pack()
+        payload += self._pack_counted_section(
+            self.nRigidBodies, bytes(rigid_body_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nSkeletons)
+        skeleton_data = bytearray()
         for i in range(self.nSkeletons):
-            payload += self.Skeletons[i].pack()
+            skeleton_data += self.Skeletons[i].pack()
+        payload += self._pack_counted_section(
+            self.nSkeletons, bytes(skeleton_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nAssets)
+        asset_data = bytearray()
         for i in range(self.nAssets):
-            payload += self.Assets[i].pack()
+            asset_data += self.Assets[i].pack()
+        payload += self._pack_counted_section(
+            self.nAssets, bytes(asset_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nLabeledMarkers)
+        labeled_marker_data = bytearray()
         for i in range(self.nLabeledMarkers):
-            payload += self.LabeledMarkers[i].pack()
+            labeled_marker_data += self.LabeledMarkers[i].pack()
+        payload += self._pack_counted_section(
+            self.nLabeledMarkers, bytes(labeled_marker_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nForcePlates)
+        force_plate_data = bytearray()
         for i in range(self.nForcePlates):
-            payload += self.ForcePlates[i].pack()
+            force_plate_data += self.ForcePlates[i].pack()
+        payload += self._pack_counted_section(
+            self.nForcePlates, bytes(force_plate_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
-        payload += struct.pack('<i', self.nDevices)
+        device_data = bytearray()
         for i in range(self.nDevices):
-            payload += self.Devices[i].pack()
+            device_data += self.Devices[i].pack()
+        payload += self._pack_counted_section(
+            self.nDevices, bytes(device_data), natnet_major=natnet_major, natnet_minor=natnet_minor
+        )
 
         # NatNet 3.0+ Timecodes & stamps
         payload += struct.pack('<II', self.Timecode, self.TimecodeSubframe)
@@ -220,4 +255,9 @@ def pack(self) -> bytes:
         payload += struct.pack('<II', self.PrecisionTimestampSecs, self.PrecisionTimestampFractionalSecs)
         payload += struct.pack('<h', self.params)
 
-        return bytes(payload)   
\ No newline at end of file
+        # End-of-data tag. libNatNet's frame unpacker reads a trailing 4-byte
+        # tag after params; without it the unpacked size mismatches nDataBytes
+        # and the SDK silently drops the whole frame (no frame callback fires).
+        payload += struct.pack('<i', 0)
+
+        return bytes(payload)
\ No newline at end of file
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
index 049d05dff..3795afd57 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
@@ -119,3 +119,18 @@ def pack(self) -> bytes:
         for i in range(self.nDataDescriptions):
             payload += self.arrDataDescriptions[i].pack()
         return bytes(payload)
+
+
+def make_default_drone_catalog() -> sDataDescriptions:
+    """Build the default single-body catalog (Drone id=1) for natnet_ros2."""
+    descriptions = sDataDescriptions()
+    descriptions.nDataDescriptions = 1
+    desc = descriptions.arrDataDescriptions[0]
+    desc.type = int(DataDescriptors.Descriptor_RigidBody)
+    rb = desc.RigidBodyDescription
+    rb.szName = b"Drone"
+    rb.ID = 1
+    rb.parentID = -1
+    rb.offsetqw = 1.0
+    rb.nMarkers = 0
+    return descriptions
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
index a7d68db87..4a186e84f 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
@@ -1,5 +1,6 @@
 from . import natnet_data_types as DataMessages
 from . import natnet_server_types as ServerMessages
+from . import natnet_model_types as ModelTypes
 from enum import Enum
 import socket
 import threading
@@ -63,6 +64,8 @@ def __init__(self,
 
         # Thread-safe queue for Mocp frames
         self.mocap_data_queue = queue.Queue(maxsize=100)
+        self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None
+        self._last_mocap_lock = threading.Lock()
         
         # Thread list and shutdown event
         self.threads = []
@@ -72,36 +75,19 @@ def __init__(self,
         self.connected_clients : typing.Set[Client] = set()
         self.clients_lock : threading.Lock = threading.Lock()
 
+        # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload)
+        self._model_def_lock = threading.Lock()
+        self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack()
+
         # Sockets
         self.command_socket : socket.socket | None = None
         self.data_socket : socket.socket | None = None
 
         self.running = False
 
-        # Start up process with threads
-        # # One thread to listen and manage command requests (TCP/UDP)
-        # # One thread to send data packets (UDP)
-        # # One thread to manage server state and data updates (mocap system or other source)
-
-        # Overall design:
-        # - On initialization, validate parameters and set up server description
-        # - Start threads for command listening and data sending
-        # - Perhaps separate into Unicast and Multicast server classes that inherit from a common base, or handle both in one class with conditional logic based on transmission type
-        # - Multicast server will need to join multicast group and manage socket options accordingly, while unicast server will send directly to client IPs
-        # - General helper functions will break packets down into appropriate sizes, serialize data messages, and manage client connections for unicast case
-        # - General main loop will manage server lifecycle and clean shutdown, while threads will handle their respective tasks for command listening and data sending
-        # - General main data management function will take in new mocap data (e.g. from mocap system or other source), update the latest data state, and trigger packet sending to clients at regular intervals (e.g. 100Hz)
-        # - Unicast:
-        # - On command request, respond with server description and data packets sent directly to requesting client's IP
-        # - Have a synchronously safe list of connected clients to manage multiple unicast recipients
-        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
-        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to unicast clients.
-        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets 
-        # - Multicast:
-        # - On command request, respond with server description but data packets will be sent to the multicast group address rather than directly to client IPs
-        # - General main data structure or queue to hold the latest mocap data that will be sent in packets to clients, with thread-safe access for updates and retrievals
-        # - Have a function to continously send packets at regular intervals (e.g. 100Hz) with the latest mocap data to the multicast group address.
-        # - - Use the general helper function to take in new mocap data (e.g. from mocap system or other source). The server will break the message down into packets and send to the multicast group address.
+        # start() launches two daemon threads: a command listener (handshake /
+        # MODELDEF / keepalive) and a data loop that streams mocap frames. The
+        # transmission-specific behavior lives in the unicast/multicast subclass.
 
     def _signal_handler(self, signum, frame):
         print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...")
@@ -116,6 +102,28 @@ def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData):
             except queue.Empty:
                 pass
         self.mocap_data_queue.put(new_data)
+        with self._last_mocap_lock:
+            self._last_mocap_frame = new_data
+
+    def _get_last_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None:
+        with self._last_mocap_lock:
+            return self._last_mocap_frame
+
+    def set_model_def_payload(self, payload: bytes) -> None:
+        """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this)."""
+        with self._model_def_lock:
+            self._model_def_payload = payload
+
+    def set_model_def_from_descriptions(
+        self, descriptions: ModelTypes.sDataDescriptions
+    ) -> None:
+        """Pack descriptions once and store as the MODELDEF wire cache."""
+        self.set_model_def_payload(descriptions.pack())
+
+    def _get_model_def_payload(self) -> bytes:
+        """Return cached MODELDEF bytes (command thread only)."""
+        with self._model_def_lock:
+            return self._model_def_payload
 
     def start(self):
         # Bind sockets and launch worker threads automatically on init
@@ -132,11 +140,15 @@ def start(self):
         self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
         self.command_socket.bind(('', self.command_port))
 
-        # 2. Setup Data Socket (Sends outward Mocap frames)
+        # 2. Setup Data Socket (Sends outward Mocap frames).
+        # Bind to the data port so frames leave with source port == data_port.
+        # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port; frames
+        # arriving from the command port are treated as command traffic and dropped.
         self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
-        # Note: Data socket doesn't need to bind to the multicast explicitly if it's only sending.
-        # It just routes via the local interface.
-        self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface))
+        self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self.data_socket.bind(('', self.data_port))
+        if self.transmission_type == TransmissionType.MULTICAST:
+            self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface))
 
         # 3. Launch Threads
         cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True)
@@ -251,15 +263,38 @@ def _build_server_description(self) -> ServerMessages.sServerDescription:
 
         return description
 
+    def _build_connect_response_payload(self) -> bytes:
+        """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server."""
+        sender = ServerMessages.sSender_Server()
+        sender.Common.szName = self._pad_fixed_string(b"Motive")
+        self._assign_version_bytes(sender.Common.Version, self.motive_app_version)
+        self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version)
+        sender.HighResClockFrequency = self.high_res_clock_freq
+        sender.DataPort = self.data_port
+        sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST
+        if self.transmission_type == TransmissionType.MULTICAST:
+            self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address)
+        else:
+            self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00")
+        return sender.pack()
+
     def _send_packet_to_client(
         self,
         client: Client,
         message_id: ServerMessages.MessageId | int,
         payload: bytes,
+        sock: socket.socket | None = None,
     ) -> None:
-        """Send a NatNet packet to a unicast client on the command socket (libNatNet 4.4)."""
-        if not self.command_socket:
-            raise ValueError("[NatNetServer] Command socket not initialized. Cannot send packet.")
+        """Send a NatNet packet to a unicast client (libNatNet 4.4).
+
+        Command replies go out the command socket; mocap frames go out the data
+        socket so their source port matches the advertised data port.
+        """
+        if self.shutdown_event.is_set():
+            return
+        sock = sock or self.command_socket
+        if not sock:
+            raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.")
 
         header = ServerMessages.sPacketHeader(
             iMessage=int(message_id),
@@ -268,7 +303,7 @@ def _send_packet_to_client(
         packet = header.pack() + payload
         try:
             with client.socket_lock:
-                self.command_socket.sendto(packet, (client.ip, client.port))
+                sock.sendto(packet, (client.ip, client.port))
         except OSError as e:
             raise ValueError(
                 f"[NatNetServer] Error sending message {int(message_id)} to "
@@ -280,7 +315,7 @@ def _data_update_loop(self): # Stub: Different betweeen multicast and unicast se
         pass
 
     def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData):
-        # Serialize frame payload and send via command socket (unicast libNatNet 4.4).
+        # Serialize frame payload and send via the data socket (unicast libNatNet 4.4).
         try:
             packet_bytes = data_message.pack()
         except Exception as e:
@@ -290,6 +325,7 @@ def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfM
             client,
             ServerMessages.MessageId.NAT_FRAMEOFDATA,
             packet_bytes,
+            sock=self.data_socket,
         )
 
     def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
index d2c1433a9..3083f1a6e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py
@@ -1,9 +1,15 @@
 import ctypes
+import struct
 from enum import IntEnum
 
 MAX_NAMELENGTH = 256
 MAX_PACKETSIZE = 65503
 
+# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1).
+SERVER_DESCRIPTION_WIRE_SIZE = 552
+# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h).
+SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4  # 279
+
 # Client/server message ids
 class MessageId(IntEnum):
     NAT_CONNECT                 = 0
@@ -24,6 +30,13 @@ class MessageId(IntEnum):
     NAT_UNRECOGNIZED_REQUEST    = 100
 
 # Server/Sender configuration and info
+def _fixed_name(field: ctypes.Array) -> bytes:
+    raw = bytes(field).split(b"\x00", 1)[0] + b"\x00"
+    if len(raw) > MAX_NAMELENGTH:
+        raw = raw[: MAX_NAMELENGTH - 1] + b"\x00"
+    return raw + b"\x00" * (MAX_NAMELENGTH - len(raw))
+
+
 class sSender(ctypes.Structure):
     _pack_ = 1
     _fields_ = [
@@ -33,7 +46,11 @@ class sSender(ctypes.Structure):
     ]
 
     def pack(self) -> bytes:
-        return bytes(self)
+        payload = bytearray()
+        payload += _fixed_name(self.szName)
+        payload += bytes(self.Version)
+        payload += bytes(self.NatNetVersion)
+        return bytes(payload)
 
 class sSender_Server(ctypes.Structure):
     _pack_ = 1
@@ -46,7 +63,15 @@ class sSender_Server(ctypes.Structure):
     ]
 
     def pack(self) -> bytes:
-        return bytes(self)
+        payload = bytearray(self.Common.pack())
+        payload += struct.pack("<QH", self.HighResClockFrequency, self.DataPort)
+        payload.append(1 if self.IsMulticast else 0)
+        payload += bytes(self.MulticastGroupAddress)
+        if len(payload) != SENDER_SERVER_WIRE_SIZE:
+            raise ValueError(
+                f"sSender_Server wire size {len(payload)} != expected {SENDER_SERVER_WIRE_SIZE}"
+            )
+        return bytes(payload)
 
 # Mocap server application description
 class sServerDescription(ctypes.Structure):
@@ -66,7 +91,30 @@ class sServerDescription(ctypes.Structure):
     ]
 
     def pack(self) -> bytes:
-        return bytes(self)
+        # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency).
+        payload = bytearray()
+        payload.append(1 if self.HostPresent else 0)
+        payload += _fixed_name(self.szHostComputerName)
+        payload += bytes(self.HostComputerAddress)
+        payload += _fixed_name(self.szHostApp)
+        payload += bytes(self.HostAppVersion)
+        payload += bytes(self.NatNetVersion)
+        while len(payload) % 8:
+            payload.append(0)
+        payload += struct.pack("<Q", self.HighResClockFrequency)
+        payload.append(1 if self.bConnectionInfoValid else 0)
+        payload.append(0)  # pad before ConnectionDataPort (offset 537)
+        payload += struct.pack("<H", self.ConnectionDataPort)
+        payload.append(1 if self.ConnectionMulticast else 0)
+        payload += bytes(self.ConnectionMulticastAddress)
+        while len(payload) < SERVER_DESCRIPTION_WIRE_SIZE:
+            payload.append(0)
+        if len(payload) != SERVER_DESCRIPTION_WIRE_SIZE:
+            raise ValueError(
+                f"sServerDescription wire size {len(payload)} != "
+                f"expected {SERVER_DESCRIPTION_WIRE_SIZE}"
+            )
+        return bytes(payload)
 
 # Base packet layout
 class sPacketHeader(ctypes.Structure):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index fb431e87f..f6da0905f 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -1,12 +1,7 @@
-from . import natnet_data_types as DataTypes
-from . import natnet_server_types as ServerTypes
-from enum import Enum
-import socket
-import threading
-import queue
-import time
-import signal
 import ctypes
+import time
+
+from . import natnet_server_types as ServerTypes
 from .natnet_server import TransmissionType, Client, NatNetServer
 
 
@@ -35,7 +30,9 @@ def _data_update_loop(self):
                 continue
 
             data_messages = self._get_latest_mocap_packet()
-            if not data_messages:
+            if data_messages is None:
+                data_messages = self._get_last_mocap_frame()
+            if data_messages is None:
                 continue
 
             for client in clients:
@@ -45,7 +42,7 @@ def _data_update_loop(self):
                     print(str(e))
                     continue
 
-    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+    def _command_listener_loop(self):
         # Listens on UDP command socket for incoming command requests from clients.
         # Handles incoming client handshakes and teardown.
 
@@ -64,7 +61,7 @@ def _command_listener_loop(self): # Stub: Different betweeen multicast and unica
                 print(f"[Command Listener] Error receiving command request: {e}")
                 time.sleep(0.1) # Sleep briefly to avoid tight loop on errors
     
-    def _handle_command_request(self, request_data: bytes, client_address: tuple): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+    def _handle_command_request(self, request_data: bytes, client_address: tuple):
         """
         Processes standard binary headers and registers unicast endpoints.
         """
@@ -96,7 +93,7 @@ def _handle_command_request(self, request_data: bytes, client_address: tuple): #
                 self._send_packet_to_client(
                     new_client,
                     ServerTypes.MessageId.NAT_SERVERINFO,
-                    self.server_description.pack(),
+                    self._build_connect_response_payload(),
                 )
             except ValueError as e:
                 raise ValueError(
@@ -118,6 +115,43 @@ def _handle_command_request(self, request_data: bytes, client_address: tuple): #
             )
             return
 
+        if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF):
+            try:
+                self._send_packet_to_client(
+                    client,
+                    ServerTypes.MessageId.NAT_MODELDEF,
+                    self._get_model_def_payload(),
+                )
+            except ValueError as e:
+                print(
+                    f"[Command Handler] Error sending MODELDEF to client "
+                    f"{client_address}: {e}"
+                )
+            return
+
+        if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE):
+            # Keep-alive is client -> server only; real Motive sends no reply.
+            # Receiving it refreshes the client's liveness; nothing to send back.
+            return
+
+        if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST):
+            echo_payload = request_data[header_size : header_size + header.nDataBytes]
+            # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes).
+            host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False)
+            response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts
+            try:
+                self._send_packet_to_client(
+                    client,
+                    ServerTypes.MessageId.NAT_ECHORESPONSE,
+                    response_payload,
+                )
+            except ValueError as e:
+                print(
+                    f"[Command Handler] Error sending ECHORESPONSE to client "
+                    f"{client_address}: {e}"
+                )
+            return
+
         print(
             f"[Command Handler] Unhandled message id {header.iMessage} from "
             f"registered client {client_address}."

From 431cbe807bb7100512b713688e33deb711c1c983 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Mon, 8 Jun 2026 12:40:02 -0400
Subject: [PATCH 08/27] test(natnet-emulator): unit tests for serializers,
 unicast protocol, defaults

Cover the MODELDEF/frame serializers, the unicast handshake protocol, and the
defaults/server catalog. A package-level test/conftest.py puts the extension
root on sys.path and registers the `unit` marker so `pytest test/` runs the
suite directly without per-file boilerplate.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .../test/conftest.py                          | 26 +++++++
 .../test/natnet_test_helpers.py               | 16 ++--
 .../test/test_defaults.py                     | 26 +++++++
 .../test/test_serializers.py                  | 37 ++++++---
 .../test/test_server_catalog.py               | 78 +++++++++++++++++++
 .../test/test_unicast_protocol.py             | 71 +++++++++++++----
 6 files changed, 219 insertions(+), 35 deletions(-)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
new file mode 100644
index 000000000..1e4eeec76
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
@@ -0,0 +1,26 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Make the emulator package importable when running these tests directly.
+
+The emulator is an Isaac Sim extension, not a pip-installed package, so a direct
+``pytest test/`` (or ``colcon test``) needs the extension root on ``sys.path``.
+pytest auto-loads this conftest before collecting any test in this directory, so
+the test modules themselves stay free of ``sys.path`` boilerplate. (Runs via
+``pytest tests/`` use the proxies under ``tests/`` instead, which set this up in
+``tests/conftest.py``.)
+"""
+
+import sys
+from pathlib import Path
+
+_EXT_ROOT = Path(__file__).resolve().parents[1]
+if str(_EXT_ROOT) not in sys.path:
+    sys.path.insert(0, str(_EXT_ROOT))
+
+
+def pytest_configure(config):
+    # Mirror the `unit` marker from tests/pytest.ini so direct `pytest test/`
+    # runs don't emit PytestUnknownMarkWarning and `-m unit` works here too.
+    config.addinivalue_line(
+        "markers", "unit: Fast hermetic unit test (no Docker stack)."
+    )
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
index 3e601f01c..07d6c8a5e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py
@@ -4,14 +4,8 @@
 
 import socket
 import struct
-import sys
 import time
 from contextlib import contextmanager
-from pathlib import Path
-
-_EXT_ROOT = Path(__file__).resolve().parents[1]
-if str(_EXT_ROOT) not in sys.path:
-    sys.path.insert(0, str(_EXT_ROOT))
 
 from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
 from optitrack.natnet.emulator.server import natnet_server_types as st
@@ -86,13 +80,21 @@ def running_unicast_server(
     local_interface: str = "127.0.0.1",
     publish_rate: int = 100,
 ):
-    """Start NatNetUnicastServer on an ephemeral or fixed command port."""
+    """Start NatNetUnicastServer on ephemeral (or fixed) command + data ports.
+
+    Both ports are ephemeral by default so concurrent/sequential tests never
+    collide on the well-known 1510/1511 pair.
+    """
     port = command_port if command_port is not None else ephemeral_udp_port(local_interface)
+    data_port = ephemeral_udp_port(local_interface)
+    while data_port == port:
+        data_port = ephemeral_udp_port(local_interface)
     server = NatNetUnicastServer(
         local_interface=local_interface,
         transmission_type=TransmissionType.UNICAST,
         multicast_address=None,
         command_port=port,
+        data_port=data_port,
     )
     server.publish_rate = publish_rate
     server.start()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py
new file mode 100644
index 000000000..7cc72b036
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py
@@ -0,0 +1,26 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Unit tests for hardcoded tracked-body defaults."""
+
+from __future__ import annotations
+
+import pytest
+
+from optitrack.natnet.emulator.defaults import (
+    DEFAULT_DRONE_BINDING,
+    DEFAULT_TRACKED_BODY_BINDINGS,
+)
+
+
+pytestmark = pytest.mark.unit
+
+
+def test_default_drone_binding_matches_natnet_ros2_config():
+    assert DEFAULT_DRONE_BINDING.name == "Drone"
+    assert DEFAULT_DRONE_BINDING.id == 1
+    assert DEFAULT_DRONE_BINDING.parent_id == -1
+    assert DEFAULT_DRONE_BINDING.prim_path == "/World/base_link"
+
+
+def test_default_tracked_body_bindings_contains_drone_only():
+    assert DEFAULT_TRACKED_BODY_BINDINGS == (DEFAULT_DRONE_BINDING,)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
index f2c9a8786..0db5a34df 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py
@@ -6,15 +6,9 @@
 
 import ctypes
 import struct
-import sys
-from pathlib import Path
 
 import pytest
 
-_EXT_ROOT = Path(__file__).resolve().parents[1]
-if str(_EXT_ROOT) not in sys.path:
-    sys.path.insert(0, str(_EXT_ROOT))
-
 from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
 from optitrack.natnet.emulator.server import natnet_data_types as dt
 from optitrack.natnet.emulator.server import natnet_model_types as mt
@@ -66,8 +60,9 @@ def test_empty_frame_pack_length_is_stable():
 
     packed = frame.pack()
 
-    # 9 count fields + timestamps + params; all collections empty.
-    assert len(packed) == 86
+    # count fields + byte-count fields (NatNet 4.4) + timestamps + params + 4-byte
+    # end-of-data tag (libNatNet's frame unpacker requires the trailing tag).
+    assert len(packed) == 122
     assert packed == frame.pack()
 
 
@@ -83,8 +78,9 @@ def test_single_rigid_body_frame_contains_id_and_params():
 
     packed = frame.pack()
 
-    assert len(packed) == 86 + 38
-    body_offset = 4 + 4 + 4 + 4  # iFrame, nMarkerSets, nOtherMarkers, nRigidBodies
+    assert len(packed) == 122 + 38
+    # iFrame + markersets + other markers (count+size each), then rigid-body count+size.
+    body_offset = 4 + 2 * 8 + 4 + 4
     body_id, = struct.unpack_from("<i", packed, body_offset)
     params_offset = body_offset + 4 + 8 * 4  # id + 8 floats (incl. MeanError)
     params, = struct.unpack_from("<h", packed, params_offset)
@@ -107,7 +103,7 @@ def test_frame_pack_negative_collection_counts_encode_without_validation():
 
     packed = frame.pack()
 
-    n_rigid_bodies, = struct.unpack_from("<i", packed, 12)
+    n_rigid_bodies, = struct.unpack_from("<i", packed, 20)
     assert n_rigid_bodies == -1
 
 
@@ -116,6 +112,22 @@ def test_frame_pack_negative_collection_counts_encode_without_validation():
 # =============================================================================
 
 
+def test_connect_response_uses_sender_server_wire_format():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=1510,
+        data_port=1511,
+    )
+    packed = server._build_connect_response_payload()
+
+    assert len(packed) == st.SENDER_SERVER_WIRE_SIZE
+    assert packed[256:260] == bytes([3, 1, 0, 0])
+    assert packed[260:264] == bytes([4, 4, 0, 0])
+    assert packed.startswith(b"Motive")
+
+
 def test_server_description_pack_includes_connection_fields():
     server = NatNetUnicastServer(
         local_interface="127.0.0.1",
@@ -127,7 +139,8 @@ def test_server_description_pack_includes_connection_fields():
     description = server.server_description
     packed = description.pack()
 
-    assert len(packed) == ctypes.sizeof(st.sServerDescription)
+    assert len(packed) == st.SERVER_DESCRIPTION_WIRE_SIZE
+    assert packed[521:525] == bytes([4, 4, 0, 0])
     assert description.HostPresent is True
     assert description.bConnectionInfoValid is True
     assert description.ConnectionDataPort == 1511
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py
new file mode 100644
index 000000000..dd54fb8e6
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py
@@ -0,0 +1,78 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Unit tests for server MODELDEF payload cache."""
+
+from __future__ import annotations
+
+import struct
+
+import pytest
+
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType, make_default_drone_catalog
+from optitrack.natnet.emulator.server import natnet_model_types as mt
+
+
+pytestmark = pytest.mark.unit
+
+
+def test_make_default_drone_catalog_packs_drone_body():
+    catalog = make_default_drone_catalog()
+    payload = catalog.pack()
+
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    desc_type, size_bytes = struct.unpack_from("<ii", payload, 4)
+    body = payload[12 : 12 + size_bytes]
+
+    assert n_descriptions == 1
+    assert desc_type == int(mt.DataDescriptors.Descriptor_RigidBody)
+    assert body.startswith(b"Drone\x00")
+
+
+def test_server_init_loads_default_drone_modeldef_payload():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+    )
+    payload = server._get_model_def_payload()
+
+    assert payload == make_default_drone_catalog().pack()
+    n_descriptions, = struct.unpack_from("<i", payload, 0)
+    assert n_descriptions == 1
+
+
+def test_set_model_def_payload_replaces_cache():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+    )
+
+    updated = mt.sDataDescriptions()
+    updated.nDataDescriptions = 1
+    desc = updated.arrDataDescriptions[0]
+    desc.type = int(mt.DataDescriptors.Descriptor_RigidBody)
+    desc.RigidBodyDescription.szName = b"Target"
+    desc.RigidBodyDescription.ID = 2
+    desc.RigidBodyDescription.nMarkers = 0
+    custom_payload = updated.pack()
+
+    server.set_model_def_payload(custom_payload)
+    payload = server._get_model_def_payload()
+
+    assert payload == custom_payload
+    body = payload[12:]
+    assert body.startswith(b"Target\x00")
+
+
+def test_set_model_def_from_descriptions_packs_once():
+    server = NatNetUnicastServer(
+        local_interface="127.0.0.1",
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+    )
+
+    catalog = make_default_drone_catalog()
+    server.set_model_def_from_descriptions(catalog)
+
+    assert server._get_model_def_payload() == catalog.pack()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
index 8d3ab10df..0414e7f2f 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
@@ -7,16 +7,10 @@
 import ctypes
 import socket
 import struct
-import sys
 import time
-from pathlib import Path
 
 import pytest
 
-_TEST_DIR = Path(__file__).resolve().parent
-if str(_TEST_DIR) not in sys.path:
-    sys.path.insert(0, str(_TEST_DIR))
-
 from natnet_test_helpers import NatNetTestClient, running_unicast_server
 
 from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType
@@ -43,19 +37,21 @@ def test_nat_connect_receives_serverinfo_on_command_port():
 
         assert addr[1] == command_port
         assert message_id == int(st.MessageId.NAT_SERVERINFO)
-        assert len(payload) == ctypes.sizeof(st.sServerDescription)
-
-        description = st.sServerDescription.from_buffer_copy(payload)
-        assert description.HostPresent is True
-        assert description.ConnectionDataPort == server.data_port
+        assert len(payload) == st.SENDER_SERVER_WIRE_SIZE
+        assert payload[256:260] == bytes([3, 1, 0, 0])
+        assert payload[260:264] == bytes([4, 4, 0, 0])
+        assert b"Motive" in payload
 
 
 # =============================================================================
-# Frame streaming — NAT_FRAMEOFDATA on command socket (libNatNet 4.4 unicast)
+# Frame streaming — NAT_FRAMEOFDATA on the data port (libNatNet 4.4 unicast)
 # =============================================================================
 
 
-def test_enqueued_frame_arrives_on_command_port():
+def test_enqueued_frame_arrives_on_data_port():
+    # libNatNet routes unicast frames by the server's data port, so frames must
+    # be sent from the data socket (source port == data_port), not the command
+    # socket. Frames sent from the command port are silently dropped by the SDK.
     with running_unicast_server(publish_rate=200) as (_server, command_port):
         client = NatNetTestClient(timeout=3.0)
         try:
@@ -74,13 +70,14 @@ def test_enqueued_frame_arrives_on_command_port():
                 except socket.timeout:
                     break
                 if message_id == int(st.MessageId.NAT_FRAMEOFDATA):
-                    assert addr[1] == command_port
+                    assert addr[1] == _server.data_port
+                    assert addr[1] != command_port
                     got_frame = True
                     break
         finally:
             client.close()
 
-        assert got_frame, "Expected NAT_FRAMEOFDATA on the command socket"
+        assert got_frame, "Expected NAT_FRAMEOFDATA from the data port"
 
 
 # =============================================================================
@@ -203,7 +200,49 @@ def test_lie_about_payload_length_in_header_still_handshake_on_connect():
 
         assert addr[1] == command_port
         assert message_id == int(st.MessageId.NAT_SERVERINFO)
-        assert len(payload) == ctypes.sizeof(st.sServerDescription)
+        assert len(payload) == st.SENDER_SERVER_WIRE_SIZE
+
+
+# =============================================================================
+# MODELDEF + KEEPALIVE — registered client happy path
+# =============================================================================
+
+
+def test_request_modeldef_after_connect_returns_drone_payload():
+    with running_unicast_server() as (_server, command_port):
+        client = NatNetTestClient(timeout=2.0)
+        try:
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            client.recv_message()
+
+            client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF)
+            message_id, payload, addr = client.recv_message()
+        finally:
+            client.close()
+
+        assert addr[1] == command_port
+        assert message_id == int(st.MessageId.NAT_MODELDEF)
+        assert payload.startswith(struct.pack("<i", 1))
+        assert b"Drone\x00" in payload
+
+
+def test_keepalive_after_connect_gets_no_reply():
+    # Keep-alive is client -> server only; real Motive sends no reply. An echo
+    # reply makes libNatNet log "Received unrecognized message Message=10".
+    with running_unicast_server() as (server, command_port):
+        client = NatNetTestClient(timeout=0.5)
+        try:
+            client.send_message(command_port, st.MessageId.NAT_CONNECT)
+            client.recv_message()
+
+            client.send_message(command_port, st.MessageId.NAT_KEEPALIVE)
+            with pytest.raises(socket.timeout):
+                client.recv_message()
+
+            # Client stays registered and keeps receiving frames.
+            assert len(server.connected_clients) == 1
+        finally:
+            client.close()
 
 
 # =============================================================================

From 45eef75544680658e27ffd41675b245fb420711f Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Mon, 8 Jun 2026 12:40:13 -0400
Subject: [PATCH 09/27] test: centralize unit-test proxy harness and register
 integration fixture

Add repo_path() and reexport_unit_tests() to tests/conftest.py so the thin
proxy files re-export co-located package tests without hardcoded
Path(__file__).parents[N] walks or per-file sys.path boilerplate; rewrite the
robot and sim proxies to use them. Register the `integration` marker and the
robot_autonomy_stack bring-up fixture (gated behind --run-integration) for the
new integration tier.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 tests/conftest.py                             | 98 ++++++++++++++++++-
 tests/pytest.ini                              |  2 +
 .../natnet_ros2/test_natnet_ros2.py           | 24 ++---
 .../test_validation_core.py                   | 30 ++----
 .../test_defaults_and_catalog.py              | 12 +++
 .../test_serializers.py                       | 23 ++---
 .../test_unicast_protocol.py                  | 23 ++---
 7 files changed, 137 insertions(+), 75 deletions(-)
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py

diff --git a/tests/conftest.py b/tests/conftest.py
index d74742dc7..d7ff96f4c 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -1,3 +1,4 @@
+import importlib.util
 import json
 import logging
 import os
@@ -51,6 +52,43 @@
 )
 
 
+def repo_path(*parts: str) -> Path:
+    """Resolve a path relative to the repository root.
+
+    ``AIRSTACK_ROOT`` is exported by CI and defaults to the repo root locally,
+    so this is the single source of truth for cross-tree paths — no test or
+    proxy file should hardcode ``Path(__file__).parents[N]`` walks.
+    """
+    return Path(AIRSTACK_ROOT).joinpath(*parts)
+
+
+def reexport_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None:
+    """Exec co-located unit-test modules and re-export their ``test_*`` callables.
+
+    Lets the thin proxy files under ``tests/`` expose package-co-located unit
+    tests to ``pytest tests/`` (CI) and ``airstack test -m unit`` without
+    per-file ``sys.path`` boilerplate. The package root (``test_dir.parent``)
+    and the test dir are placed on ``sys.path`` so the exec'd modules can import
+    the package under test and any sibling helper modules. Every re-exported
+    test is tagged ``unit`` so selection works regardless of whether the source
+    used a module-level ``pytestmark`` or per-function marks.
+    """
+    for path in (test_dir, test_dir.parent):
+        entry = str(path)
+        if entry not in sys.path:
+            sys.path.insert(0, entry)
+    prefix = re.sub(r"\W", "_", test_dir.parent.name)
+    for module_file in module_files:
+        real_file = test_dir / module_file
+        module_name = f"_unit_{prefix}_{Path(module_file).stem}"
+        spec = importlib.util.spec_from_file_location(module_name, real_file)
+        module = importlib.util.module_from_spec(spec)
+        spec.loader.exec_module(module)
+        for name in dir(module):
+            if name.startswith("test_"):
+                target_globals[name] = pytest.mark.unit(getattr(module, name))
+
+
 def load_colcon_unit_test_config(workspace="robot"):
     """Load colcon test package list and pytest args from tests/colcon_unit_test_packages.yaml."""
     if not COLCON_UNIT_TEST_PACKAGES_YAML.is_file():
@@ -122,6 +160,11 @@ def pytest_addoption(parser):
     parser.addoption("--takeoff-velocities", default="0.5",
                      help="Comma-separated takeoff/land velocities (m/s) to "
                           "sweep in test_takeoff_hover_land. Default: 0.5,1,2")
+    parser.addoption("--run-integration", action="store_true", default=False,
+                     help="Let integration tests (tests/integration/) bring up "
+                          "the robot container themselves. Without it they reuse "
+                          "an already-running container or skip. Keeps a plain "
+                          "`pytest tests/` from spinning up Docker.")
 
 
 def _chmod_world_writable(path: Path) -> None:
@@ -814,4 +857,57 @@ def airstack_env(request):
             airstack_cmd("down", timeout=120, log_name=log)
             down_duration_s = round(time.time() - t3, 2)
             logger.info("Teardown finished in %.2fs", down_duration_s)
-        m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s")
\ No newline at end of file
+        m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s")
+
+
+# ── integration tier (tests/integration/) ─────────────────────────────────
+
+_INTEGRATION_ROBOT_PATTERN = "robot.*desktop"
+# Robot-only bring-up: autonomy stack on, no sim profile, single robot.
+_INTEGRATION_ENV = {
+    "AUTOLAUNCH": "true",
+    "NUM_ROBOTS": "1",
+    "COMPOSE_PROFILES": "desktop",
+}
+
+
+@pytest.fixture(scope="module")
+def robot_autonomy_stack(request):
+    """Robot-desktop container for integration tests (no sim, no GPU).
+
+    Yields ``{"container": <name>, "brought_up": bool}``. Reuses an already
+    running container (fast local iteration, left running afterward); otherwise
+    runs ``airstack up robot-desktop`` only when ``--run-integration`` is passed
+    (and tears it down). Without the flag and with nothing running, the test is
+    skipped, so a plain ``pytest tests/`` never spins up Docker for this tier.
+    """
+    existing = find_container(_INTEGRATION_ROBOT_PATTERN)
+    if existing and container_running(existing):
+        yield {"container": existing, "brought_up": False}
+        return
+
+    if not request.config.getoption("--run-integration"):
+        pytest.skip(
+            "No running robot-desktop container. Start one "
+            "(`AUTOLAUNCH=false airstack up robot-desktop`) or pass "
+            "`--run-integration` to let the harness bring it up."
+        )
+
+    log = "robot_autonomy_stack"
+    with logger_to(log):
+        missing = missing_images(env=_INTEGRATION_ENV)
+        if missing:
+            pytest.skip("robot-desktop image not built locally: " + ", ".join(missing))
+        airstack_cmd("down", timeout=120, log_name=log)
+        result = airstack_cmd("up", "robot-desktop",
+                              env_overrides=_INTEGRATION_ENV, timeout=180, log_name=log)
+        assert result.returncode == 0, \
+            f"`airstack up robot-desktop` failed:\n{read_log_tail(log)}"
+
+    container = wait_for_container(_INTEGRATION_ROBOT_PATTERN, timeout=120)
+    assert container, "robot-desktop container not Running after 120s"
+    try:
+        yield {"container": container, "brought_up": True}
+    finally:
+        with logger_to(log):
+            airstack_cmd("down", timeout=120, log_name=log)
\ No newline at end of file
diff --git a/tests/pytest.ini b/tests/pytest.ini
index a664ccbf3..f5220f93a 100644
--- a/tests/pytest.ini
+++ b/tests/pytest.ini
@@ -6,6 +6,8 @@ markers =
     liveliness: Container and process health (Docker, tmux, sentinel ROS 2 nodes)
     sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF
     takeoff_hover_land: End-to-end takeoff / hover / land action tests
+    integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU)
+    natnet: NatNet emulator ↔ natnet_ros2 integration (robot container + UDP server)
 testpaths = .
 addopts = -v --durations=0
 cache_dir = /tmp/.pytest_cache
diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
index fc9a78bde..aa1f2023d 100644
--- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
+++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
@@ -11,22 +11,10 @@
 gtests and ament linters.
 """
 
-import importlib.util
-import sys
-from pathlib import Path
+from conftest import reexport_unit_tests, repo_path
 
-_repo_root = Path(__file__).resolve().parents[4]
-_pkg_test = _repo_root / "robot/ros_ws/src/perception/natnet_ros2/test"
-_real_file = _pkg_test / "test_natnet_ros2.py"
-
-# Load the real module under a unique name to avoid the circular-import that
-# would occur if we used `from test_natnet_ros2 import *` (this file has the
-# same name and pytest adds its directory to sys.path at collection time).
-_spec = importlib.util.spec_from_file_location("_natnet_ros2_unit_tests", _real_file)
-_real = importlib.util.module_from_spec(_spec)
-_spec.loader.exec_module(_real)
-
-# Re-export every test_* symbol so pytest collects them from this proxy.
-for _name in dir(_real):
-    if _name.startswith("test_"):
-        globals()[_name] = getattr(_real, _name)
+reexport_unit_tests(
+    globals(),
+    repo_path("robot/ros_ws/src/perception/natnet_ros2/test"),
+    "test_natnet_ros2.py",
+)
diff --git a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
index e7babf9d4..7af5ab631 100644
--- a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
+++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
@@ -11,28 +11,10 @@
 the ament linters.
 """
 
-import importlib.util
-import sys
-from pathlib import Path
+from conftest import reexport_unit_tests, repo_path
 
-_repo_root = Path(__file__).resolve().parents[4]
-_pkg_test = _repo_root / "robot/ros_ws/src/sensors/lidar_point_cloud_filter/test"
-_pkg_root = _pkg_test.parent  # adds lidar_point_cloud_filter/ package to sys.path
-_real_file = _pkg_test / "test_validation_core.py"
-
-# Make the package module importable so the real test can do
-# `from lidar_point_cloud_filter.validation_core import ...`
-if str(_pkg_root) not in sys.path:
-    sys.path.insert(0, str(_pkg_root))
-
-# Load the real module under a unique name to avoid the circular-import that
-# would occur if we used `from test_validation_core import *` (this file has
-# the same name and pytest adds its directory to sys.path at collection time).
-_spec = importlib.util.spec_from_file_location("_lidar_validation_unit_tests", _real_file)
-_real = importlib.util.module_from_spec(_spec)
-_spec.loader.exec_module(_real)
-
-# Re-export every test_* symbol so pytest collects them from this proxy.
-for _name in dir(_real):
-    if _name.startswith("test_"):
-        globals()[_name] = getattr(_real, _name)
+reexport_unit_tests(
+    globals(),
+    repo_path("robot/ros_ws/src/sensors/lidar_point_cloud_filter/test"),
+    "test_validation_core.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py
new file mode 100644
index 000000000..0dca746d2
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py
@@ -0,0 +1,12 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator default/catalog unit tests for pytest tests/."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_defaults.py",
+    "test_server_catalog.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_serializers.py b/tests/sim/optitrack_natnet_emulator/test_serializers.py
index 58f7f3099..22226cfff 100644
--- a/tests/sim/optitrack_natnet_emulator/test_serializers.py
+++ b/tests/sim/optitrack_natnet_emulator/test_serializers.py
@@ -1,20 +1,11 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator unit tests for pytest tests/."""
+"""Proxy: re-exposes optitrack.natnet.emulator serializer unit tests for pytest tests/."""
 
-import importlib.util
-from pathlib import Path
+from conftest import reexport_unit_tests, repo_path
 
-import pytest
-
-_repo_root = Path(__file__).resolve().parents[3]
-_pkg_test = _repo_root / "simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"
-_real_file = _pkg_test / "test_serializers.py"
-
-_spec = importlib.util.spec_from_file_location("_optitrack_natnet_serializers", _real_file)
-_real = importlib.util.module_from_spec(_spec)
-_spec.loader.exec_module(_real)
-
-for _name in dir(_real):
-    if _name.startswith("test_"):
-        globals()[_name] = pytest.mark.unit(getattr(_real, _name))
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_serializers.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
index 0840b4f78..e70a25064 100644
--- a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
+++ b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
@@ -1,20 +1,11 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator unit tests for pytest tests/."""
+"""Proxy: re-exposes optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/."""
 
-import importlib.util
-from pathlib import Path
+from conftest import reexport_unit_tests, repo_path
 
-import pytest
-
-_repo_root = Path(__file__).resolve().parents[3]
-_pkg_test = _repo_root / "simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"
-_real_file = _pkg_test / "test_unicast_protocol.py"
-
-_spec = importlib.util.spec_from_file_location("_optitrack_natnet_unicast_protocol", _real_file)
-_real = importlib.util.module_from_spec(_spec)
-_spec.loader.exec_module(_real)
-
-for _name in dir(_real):
-    if _name.startswith("test_"):
-        globals()[_name] = pytest.mark.unit(getattr(_real, _name))
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_unicast_protocol.py",
+)

From 1bb514b79340e226f92762dca88669039366420b Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Mon, 8 Jun 2026 12:40:23 -0400
Subject: [PATCH 10/27] test: add integration test tier and migrate NatNet
 integration test

Introduce tests/integration/ as a new tier between unit and system: real
components wired together (robot autonomy container + a host-side component)
with no sim or GPU. Move the NatNet emulator <-> natnet_ros2 test here as the
first resident (marks: integration, natnet), driven by the shared
robot_autonomy_stack fixture, and remove the old tests/sim/motive_emulator
location.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 tests/integration/README.md                   |  53 +++++
 tests/integration/natnet/README.md            |  89 ++++++++
 .../natnet/test_natnet_integration.py         | 198 ++++++++++++++++++
 tests/sim/README.md                           |   4 +-
 tests/sim/motive_emulator/README.md           |  61 ------
 5 files changed, 343 insertions(+), 62 deletions(-)
 create mode 100644 tests/integration/README.md
 create mode 100644 tests/integration/natnet/README.md
 create mode 100644 tests/integration/natnet/test_natnet_integration.py
 delete mode 100644 tests/sim/motive_emulator/README.md

diff --git a/tests/integration/README.md b/tests/integration/README.md
new file mode 100644
index 000000000..8db63f3d1
--- /dev/null
+++ b/tests/integration/README.md
@@ -0,0 +1,53 @@
+# Integration tests (`tests/integration/`)
+
+The **integration** tier sits between **unit** and **system**:
+
+| Tier | Lives in | Brings up | Hardware | Mark |
+|------|----------|-----------|----------|------|
+| unit | `<pkg>/test/` + proxies in `tests/robot/`, `tests/sim/`, `tests/gcs/` | nothing | none | `unit` |
+| **integration** | **`tests/integration/<scenario>/`** | **robot container + a host-side component** | **Docker (no sim/GPU)** | **`integration`** (+ a specific mark) |
+| system | `tests/system/` | full sim + robot + GCS | Docker + GPU + sim license | `liveliness`, `sensors`, `takeoff_hover_land` |
+
+An integration test wires a few **real** components together — for example the
+robot autonomy stack plus a host-side NatNet server — without paying for a full
+simulator or GPU.
+
+## The harness: `robot_autonomy_stack`
+
+The shared bring-up fixture lives in the root [`../conftest.py`](../conftest.py)
+(alongside `airstack_env`, matching the repo convention that fixtures are
+defined there). Request it instead of hand-rolling `airstack up`:
+
+- **Reuses** an already-running `robot-desktop` container (fast local iteration;
+  left running afterward).
+- Otherwise brings one up (`airstack up robot-desktop`, autonomy on, no sim)
+  **only when `--run-integration` is passed**, and tears it down after.
+- Otherwise **skips** — so a plain `pytest tests/` never spins up Docker.
+
+## Running
+
+```bash
+# Reuse a container you already started:
+AUTOLAUNCH=false airstack up robot-desktop
+pytest tests/integration/ -m integration -v
+
+# Or let the harness bring the robot container up/down itself:
+pytest tests/integration/ -m integration --run-integration -v
+
+# On CI / a PR, on-demand:
+#   /pytest -m integration --run-integration
+```
+
+## Adding a scenario
+
+1. Create `tests/integration/<scenario>/test_*.py`.
+2. `pytestmark = [pytest.mark.integration, pytest.mark.<scenario>]` — the
+   `integration` umbrella selects the whole tier; the specific mark targets one.
+3. Request the `robot_autonomy_stack` fixture for the container.
+4. Register the specific mark in [`../pytest.ini`](../pytest.ini).
+
+## Residents
+
+| Scenario | Mark | What it verifies |
+|----------|------|------------------|
+| [`natnet/`](natnet/) | `natnet` | Host NatNet emulator → `natnet_ros2` → pose topic Hz |
diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md
new file mode 100644
index 000000000..8a004282a
--- /dev/null
+++ b/tests/integration/natnet/README.md
@@ -0,0 +1,89 @@
+# NatNet ↔ robot autonomy integration
+
+Drives the Python NatNet wire-protocol emulator against `natnet_ros2_node`
+running with the robot autonomy stack. First resident of the
+[`integration`](../README.md) tier.
+
+Marks: `integration` (tier) + `natnet` (scenario).
+
+## What it verifies
+
+1. Emulator serves the default Drone MODELDEF and streams frames with rigid-body `ID=1`.
+2. `natnet_ros2_node` connects via unicast, parses MODELDEF, and publishes
+   `/{ROBOT_NAME}/perception/optitrack/Drone` at ≥ 5 Hz.
+
+## Requirements
+
+- A NatNet UDP server (`NatNetUnicastServer`) on the host — started by the test.
+- `natnet_ros2_node` (OptiTrack NatNet SDK) built in the robot container.
+- Docker bridge routing from container → host gateway.
+
+The robot container is provided by the `robot_autonomy_stack` fixture (see the
+[tier README](../README.md)). The test **skips** when `natnet_ros2_node` is not
+built (the SDK is license-gated, fetched via `airstack setup --natnet`).
+
+## Running
+
+```bash
+# Reuse an existing robot container:
+AUTOLAUNCH=false airstack up robot-desktop
+pytest tests/integration/natnet/ -m natnet -v
+
+# Or let the harness bring the container up/down:
+pytest tests/integration/natnet/ -m natnet --run-integration -v
+```
+
+## Architecture
+
+```
+┌─────────────────────────────────────────────────────────────┐
+│ Host (pytest)                                                │
+│  NatNetUnicastServer — MODELDEF bytes cache + frame queue   │
+└───────────────────────────┬─────────────────────────────────┘
+                            │ UDP (docker bridge gateway IP)
+┌───────────────────────────▼─────────────────────────────────┐
+│ Robot container (autonomy stack)                             │
+│  natnet_ros2_node (libNatNet 4.4 unicast)                   │
+│  → /{ROBOT_NAME}/perception/optitrack/Drone                 │
+└─────────────────────────────────────────────────────────────┘
+```
+
+**Catalog ownership:** The server holds a MODELDEF **wire cache** only. Scene
+semantics (prim paths, body names/IDs) belong in the future Isaac Sim wrapper,
+which calls `set_model_def_payload()`. See
+[`defaults.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py)
+for hardcoded Drone reference constants used in tests.
+
+## Future: Isaac-wrapped variant + liveliness
+
+Today the NatNet **server** is the host emulator. Once the Isaac-sim emulator
+wrapper emits NatNet frames from the simulator, an Isaac-wrapped variant will be
+added in this directory, and the gated pose-rate check can additionally surface
+as a conditional sentinel in
+[`../../system/test_liveliness.py`](../../system/test_liveliness.py) (run only
+when `LAUNCH_NATNET=true`).
+
+## libNatNet 4.4 unicast — verified wire contract
+
+The emulator is validated against the **real `libNatNet.so`** (not just the Python
+`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback`
+and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver
+frames to the callback:
+
+| Requirement | Why |
+|-------------|-----|
+| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` |
+| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert |
+| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** |
+| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback |
+| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` |
+
+With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`,
+`data descriptions: 1`, and **~74 Hz** of frame callbacks.
+
+> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag
+> and *on the command port*, which is why it appeared to work while libNatNet did
+> not. Always validate against the C SDK.
+
+Rebuild `natnet_ros2` in the robot container after any adapter changes:
+`docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2'`
diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py
new file mode 100644
index 000000000..b95376797
--- /dev/null
+++ b/tests/integration/natnet/test_natnet_integration.py
@@ -0,0 +1,198 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""NatNet emulator ↔ robot autonomy integration test.
+
+Wide-scale integration: a host-side NatNet server (the Python emulator) streams
+dummy Drone frames to ``natnet_ros2`` running against the robot autonomy stack,
+and we assert the pose topic stays alive at a stable rate.
+
+This is the first resident of the ``integration`` tier. Today the NatNet
+*server* is the host emulator; once the Isaac-sim emulator wrapper emits NatNet
+frames, an Isaac-wrapped variant will be added here, and the gated pose-rate
+check can additionally surface in ``system/test_liveliness.py``.
+"""
+
+from __future__ import annotations
+
+import subprocess
+import sys
+import threading
+import time
+
+import pytest
+
+from conftest import (  # noqa: E402 — pytest adds tests/ to sys.path
+    docker_exec,
+    repo_path,
+    ros2_env,
+    sample_hz,
+    wait_for_first_message,
+)
+
+# Emulator package (host-side) is not pip-installed; expose it + its test
+# helpers via the AIRSTACK_ROOT-anchored repo_path() (works in CI and locally).
+_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator")
+for _path in (_EXT_ROOT, _EXT_ROOT / "test"):
+    if str(_path) not in sys.path:
+        sys.path.insert(0, str(_path))
+
+from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType  # noqa: E402
+from optitrack.natnet.emulator.server import natnet_data_types as dt  # noqa: E402
+from natnet_test_helpers import ephemeral_udp_port  # noqa: E402
+
+pytestmark = [pytest.mark.integration, pytest.mark.natnet]
+
+_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash"
+_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node"
+_WARMUP_S = 2.0
+_STREAM_HOLD_S = 12.0
+_MIN_HZ = 5.0
+
+# Robot image has route/netstat but not `ip`; /proc/net/route is always present.
+_DEFAULT_GATEWAY_CMD = (
+    """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """
+    """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """
+    """/proc/net/route"""
+)
+
+
+def _docker_default_gateway(container: str) -> str:
+    result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10)
+    gateway = result.stdout.strip()
+    if not gateway:
+        pytest.skip(f"Could not resolve default gateway inside {container}")
+    return gateway
+
+
+def _container_env(container: str, var: str, default: str) -> str:
+    # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV.
+    # .bashrc may print "Sourcing ..." to stdout; take the last line as the value.
+    result = docker_exec(container, f"bash -lc 'echo ${var}'")
+    lines = [line.strip() for line in result.stdout.splitlines() if line.strip()]
+    value = lines[-1] if lines else ""
+    return value if value else default
+
+
+def _natnet_node_available(container: str) -> bool:
+    result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no")
+    return "yes" in result.stdout
+
+
+def _stop_stale_natnet_nodes(container: str) -> None:
+    docker_exec(container, "pkill -f natnet_ros2_node || true")
+    time.sleep(0.5)
+
+
+def _make_drone_frame(frame_num: int) -> dt.sFrameOfMocapData:
+    frame = dt.sFrameOfMocapData()
+    frame.iFrame = frame_num
+    frame.nRigidBodies = 1
+    rb = frame.RigidBodies[0]
+    rb.ID = 1
+    rb.qw = 1.0
+    # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp).
+    rb.params = 1
+    return frame
+
+
+def _frame_publisher(server: NatNetUnicastServer, stop_event: threading.Event) -> None:
+    frame_num = 0
+    interval = 1.0 / server.publish_rate
+    while not stop_event.is_set():
+        server.enqueue_mocap_data(_make_drone_frame(frame_num))
+        frame_num += 1
+        time.sleep(interval)
+
+
+def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
+    """Emulator on host streams dummy Drone frames while natnet_ros2_node publishes."""
+    container = robot_autonomy_stack["container"]
+
+    if not _natnet_node_available(container):
+        pytest.skip(
+            "natnet_ros2_node not built — run airstack setup (NatNet SDK) and "
+            "bws --packages-select natnet_ros2 in the robot container"
+        )
+
+    _stop_stale_natnet_nodes(container)
+
+    host_ip = _docker_default_gateway(container)
+    command_port = ephemeral_udp_port(host_ip)
+    robot_name = _container_env(container, "ROBOT_NAME", "robot_1")
+    domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0"))
+    pose_topic = f"/{robot_name}/perception/optitrack/Drone"
+    pose_cov_topic = f"{pose_topic}/pose_cov"
+
+    server = NatNetUnicastServer(
+        local_interface=host_ip,
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=command_port,
+    )
+    server.publish_rate = 50
+
+    stop_event = threading.Event()
+    publisher = threading.Thread(
+        target=_frame_publisher,
+        args=(server, stop_event),
+        daemon=True,
+    )
+
+    node_proc: subprocess.Popen[str] | None = None
+    try:
+        # Seed dummy frames before the client connects; keep streaming for the full hold window.
+        publisher.start()
+        time.sleep(0.1)
+        server.start()
+
+        launch_cmd = (
+            f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && "
+            f"exec {_NATNET_NODE} --ros-args "
+            f"-p server_ip:={host_ip} "
+            f"-p command_port:={command_port} "
+            f"-p body_name:=Drone "
+            f"-p body_id:=1 "
+            f"-p publish_to_mavros:=false "
+            f"-p publish_direct_optitrack:=true'"
+        )
+        node_proc = subprocess.Popen(
+            ["docker", "exec", container, "bash", "-c", launch_cmd],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.STDOUT,
+            text=True,
+        )
+
+        time.sleep(_WARMUP_S)
+        first_msg_s = wait_for_first_message(
+            container,
+            pose_cov_topic,
+            domain_id,
+            _ROBOT_SETUP,
+            timeout=int(_STREAM_HOLD_S),
+        )
+        assert first_msg_s is not None, (
+            f"No messages on {pose_cov_topic} within {_STREAM_HOLD_S}s "
+            "(NatNet connect or frame stream failed)"
+        )
+
+        # Server still streaming — measure sustained rate over the remaining hold window.
+        hz = sample_hz(
+            container,
+            pose_topic,
+            domain_id,
+            _ROBOT_SETUP,
+            duration=min(8, int(_STREAM_HOLD_S - first_msg_s)),
+            window=20,
+        )
+        assert hz is not None, f"No sustained stream on {pose_topic}"
+        assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}"
+    finally:
+        stop_event.set()
+        publisher.join(timeout=2.0)
+        if node_proc is not None:
+            node_proc.terminate()
+            try:
+                node_proc.wait(timeout=5)
+            except subprocess.TimeoutExpired:
+                node_proc.kill()
+        server.shutdown()
diff --git a/tests/sim/README.md b/tests/sim/README.md
index 09f45f6a6..880930d31 100644
--- a/tests/sim/README.md
+++ b/tests/sim/README.md
@@ -6,9 +6,11 @@ AirSim bridge utilities).
 
 Mark fast, hermetic checks with `@pytest.mark.unit`. Tests that require a GPU,
 full sim, or Docker belong in [`tests/system/`](../system/) instead.
+Cross-component tests that need the robot container (but not a sim) belong in
+[`tests/integration/`](../integration/) (mark: `integration`).
 
 Suggested layout:
 
 | Directory | Purpose |
 |-----------|---------|
-| `motive_emulator/` | Motive / NatNet protocol emulation / parsing |
+| `optitrack_natnet_emulator/` | NatNet emulator unit tests (proxy; mark: `unit`) |
diff --git a/tests/sim/motive_emulator/README.md b/tests/sim/motive_emulator/README.md
deleted file mode 100644
index 0e682c448..000000000
--- a/tests/sim/motive_emulator/README.md
+++ /dev/null
@@ -1,61 +0,0 @@
-# Motive / NatNet Emulator
-
-This directory is the future home of **integration tests** that drive a real
-NatNet wire-protocol mock server against `natnet_ros2_node`.
-
-## Why here, not in the package test/ dir?
-
-Unit tests for pure logic live in
-`tests/robot/perception/natnet_ros2/test_natnet_logic.cpp` and run via `colcon
-test` with no network or SDK required (uses `FakeNatNetClient`).
-
-The emulator tests here will require an actual UDP server that speaks the NatNet
-protocol, so they belong in the `sensors` mark of the system test suite alongside
-other topic-streaming tests.
-
-## Planned implementation
-
-The mock server should:
-
-1. Open a UDP socket on the NatNet command port (default 1510).
-2. Respond to `NAT_CONNECT` (message type 0) with a `NAT_SERVERINFO` (type 1)
-   packet containing a canned `sServerDescription`.
-3. Respond to `NAT_REQUEST_MODELDEF` (type 4) with a `NAT_MODELDEF` (type 5)
-   packet describing one or more rigid bodies.
-4. Stream `NAT_FRAMEOFDATA` (type 7) packets to the client's data port at a
-   configurable rate with synthetic pose data.
-
-### Reference
-
-The NatNet wire format is documented in the NatNet SDK developer notes and the
-`PacketClient` example shipped with the SDK (available inside the robot Docker
-container after `airstack setup --natnet`).
-
-## Relationship to `FakeNatNetClient`
-
-```
-                ┌──────────────────────────────────────┐
-                │         Test boundary                 │
- colcon gtest   │  FakeNatNetClient (in-process)        │  ← unit tests (no network)
-                │  test_natnet_logic.cpp                │
-                └──────────────────────────────────────┘
-
-                ┌──────────────────────────────────────┐
-                │         Network boundary              │
- pytest sensors │  MotiveEmulator (UDP server, Python)  │  ← integration tests
-                │  NatNetClientAdapter → NatNetClient   │
-                │  natnet_ros2_node (full ROS node)     │
-                └──────────────────────────────────────┘
-```
-
-The `FakeNatNetClient` seam (already implemented) lets unit tests verify all
-connection-outcome logic paths.  The emulator here will verify the full
-end-to-end path including the NatNet SDK's own parser.
-
-## When to add this
-
-Implement the emulator when:
-- The OptiTrack emulator service is placed under `simulation/optitrack-emulator/`
-  or `tests/sim/motive_emulator/`
-- The `sensors` test mark is extended to include `natnet_ros2` topic checks
-- CI has access to the robot container with the NatNet SDK installed

From 0fa88e45c61c1f396b115f64c1e4180a6e3b3488 Mon Sep 17 00:00:00 2001
From: airlab <johnliuchs2022@gmail.com>
Date: Mon, 8 Jun 2026 12:40:33 -0400
Subject: [PATCH 11/27] docs: document test tiers, proxy harness, and NatNet
 integration path

Update AGENTS.md, tests/README.md, and the add-unit-tests / optitrack-development
skills to describe the four test tiers (unit, integration, system), the
repo_path/reexport_unit_tests proxy pattern, the --run-integration flag, and the
new tests/integration/natnet location.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .agents/skills/add-unit-tests/SKILL.md        | 52 ++++++++---------
 .agents/skills/optitrack-development/SKILL.md | 58 +++++++++++--------
 AGENTS.md                                     |  6 +-
 .../intermediate/testing/unit_testing.md      |  4 +-
 tests/README.md                               | 29 +++++++++-
 5 files changed, 89 insertions(+), 60 deletions(-)

diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md
index 7d1d3b582..2ab4234fe 100644
--- a/.agents/skills/add-unit-tests/SKILL.md
+++ b/.agents/skills/add-unit-tests/SKILL.md
@@ -114,7 +114,11 @@ For `rclpy.node.Node` subclasses use a real dummy base class instead of a
 
 ### 3. Write the thin proxy in tests/robot/
 
-Create `tests/robot/<layer>/<package>/test_<name>.py`:
+Create `tests/robot/<layer>/<package>/test_<name>.py`. Use the shared
+`reexport_unit_tests` + `repo_path` helpers from `tests/conftest.py` so the proxy
+stays a two-call shim and the cross-tree path is anchored on `AIRSTACK_ROOT`
+(exported by CI, defaults to the repo root locally) — **never** count
+`Path(__file__).parents[N]` or hardcode `sys.path` walks in the proxy:
 
 ```python
 # Copyright (c) 2024 Carnegie Mellon University
@@ -127,37 +131,27 @@ Unit test logic lives co-located with the package source (ROS 2 / colcon convent
 This file makes those tests discoverable by ``pytest tests/`` (CI) and
 ``airstack test -m unit`` without any changes to the CI workflow.
 """
-import importlib.util
-import sys
-from pathlib import Path
-
-_repo_root = Path(__file__).resolve().parents[N]   # adjust N so this resolves to repo root
-_pkg_test = _repo_root / "robot/ros_ws/src/<layer>/<package>/test"
-_real_file = _pkg_test / "test_<name>.py"
-
-# If the test imports from a package module, ensure the package root is on sys.path.
-# Example: _pkg_root = _pkg_test.parent; sys.path.insert(0, str(_pkg_root))
+from conftest import reexport_unit_tests, repo_path
 
-# Load the real module under a unique name to avoid the circular import that
-# would occur if we used `from test_<name> import *` (this file has the same
-# name, and pytest adds its directory to sys.path at collection time).
-_spec = importlib.util.spec_from_file_location("_<package>_unit_tests", _real_file)
-_real = importlib.util.module_from_spec(_spec)
-_spec.loader.exec_module(_real)
-
-# Re-export every test_* symbol so pytest collects them from this proxy.
-for _name in dir(_real):
-    if _name.startswith("test_"):
-        globals()[_name] = getattr(_real, _name)
+reexport_unit_tests(
+    globals(),
+    repo_path("robot/ros_ws/src/<layer>/<package>/test"),
+    "test_<name>.py",   # pass several filenames to fold multiple modules into one proxy
+)
 ```
 
-**Counting `parents[N]` to reach the repo root:**
-
-| Proxy location | `parents[N]` for repo root |
-|---|---|
-| `tests/robot/<layer>/<package>/` | `parents[4]` |
-| `tests/sim/<tool>/` | `parents[3]` |
-| `tests/gcs/<package>/` | `parents[3]` |
+`reexport_unit_tests` (in `tests/conftest.py`) execs each co-located module with
+`importlib` under a unique name (avoiding the same-filename circular import), puts
+both the test dir and its parent (the package root) on `sys.path` so the source
+can import its package and sibling helpers, and tags every re-exported `test_*`
+with `pytest.mark.unit`. Because the root `conftest` is imported before any proxy
+is collected, `from conftest import ...` resolves in both CI and local runs.
+
+For a **direct** `pytest <package>/test/` (or `colcon test`) run — which bypasses
+the proxies — add a tiny `conftest.py` in the package `test/` dir that puts the
+package/extension root on `sys.path` (see
+`simulation/.../optitrack.natnet.emulator/test/conftest.py`). The test source
+then stays free of `sys.path` boilerplate.
 
 ### 4. Ensure the tests/ directory structure exists
 
diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md
index 42a561830..0aa8fbc04 100644
--- a/.agents/skills/optitrack-development/SKILL.md
+++ b/.agents/skills/optitrack-development/SKILL.md
@@ -39,7 +39,7 @@ flowchart LR
 | Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) |
 | SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) |
 | Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests |
-| Planned integration tests | [`tests/sim/motive_emulator/README.md`](../../../tests/sim/motive_emulator/README.md) | End-to-end UDP tests against real SDK parser |
+| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (marks: `integration`, `natnet`) |
 
 **Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`.
 
@@ -50,12 +50,14 @@ flowchart LR
 | Port (server default) | Channel | Direction |
 |----------------------|---------|-----------|
 | **1510** | Command | Client → server: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives. Server → client: `NAT_SERVERINFO`, `NAT_MODELDEF`, `NAT_RESPONSE` |
-| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **libNatNet 4.4 unicast clients receive frames on the same socket/port as command traffic** (see below). |
+| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **The server must send frames from a socket bound to the data port** (source port == `data_port`); see below. |
 
-**Critical rules (verified against libNatNet 4.4 unicast):**
+**Critical rules (verified against the real `libNatNet.so` 4.4 unicast + `NatNet_SetLogCallback`):**
 
-- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`).
-- **libNatNet 4.4 unicast uses one client UDP socket** — command send, command receive, and frame receive all share the **same ephemeral local port**. Do **not** assume `data_port = cmd_port + 1`.
+- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`), sent via the **command** socket.
+- **Frames must be sent from the server's DATA socket** (bound to `data_port`, e.g. `1511`) so the datagram **source port == `data_port`**. libNatNet routes inbound unicast datagrams by source port: frames from the **command** port are treated as command traffic and **silently dropped** (no error, no callback). This was the single biggest gotcha.
+- **libNatNet 4.4 unicast uses one client UDP socket** (one ephemeral local port for command send/recv and frame recv). The client receives frames there regardless of the server's source port — but libNatNet only **dispatches** them to the frame callback when they came from the server's data port. Do **not** assume `data_port = cmd_port + 1`.
+- **Every `NAT_FRAMEOFDATA` must end with a 4-byte end-of-data tag** (after the frame `params`). libNatNet's unpacker reads it; without it the unpacked length mismatches `nDataBytes` and the SDK drops the whole frame. (The lenient Python `NatNetClient` does not require it — always validate against the C SDK.)
 - The **269-byte `NAT_CONNECT` payload does not include** the client port; the port is learned from the datagram **source address** on `NAT_CONNECT`.
 - Do **not** trust `/proc`/`ss` alone for the client port — extra bound sockets may appear that do not match wire traffic. **`NAT_CONNECT` source `(ip, port)` is ground truth.**
 - Do **not** parse connect payloads with in-memory `sNatNetClientConnectParams` (contains pointers). Use on-wire layouts below.
@@ -107,15 +109,19 @@ For libNatNet 4.4 unicast, treat the client as **single-endpoint**:
 
 ```text
 On NAT_CONNECT     → store client_endpoint = (ip, port) from recvfrom
-NAT_SERVERINFO     → sendto(command_socket, client_endpoint)
-NAT_MODELDEF       → sendto(command_socket, client_endpoint)
-NAT_FRAMEOFDATA    → sendto(command_socket, client_endpoint)   # same port, not cmd+1
-NAT_KEEPALIVE ack  → sendto(command_socket, client_endpoint)
+NAT_SERVERINFO     → sendto(command_socket, client_endpoint)   # source port = command_port
+NAT_MODELDEF       → sendto(command_socket, client_endpoint)   # source port = command_port
+NAT_FRAMEOFDATA    → sendto(data_socket,    client_endpoint)   # source port = data_port  (REQUIRED)
+NAT_KEEPALIVE      → no reply (client -> server only)
 ```
 
-The server still **binds** command (`1510`) and data (`1511`) ports per NatNet convention, but **`natnet_ros2` does not expose a separate client data port** — stream frames to the **`NAT_CONNECT` source address** from the **command socket**.
+The client always learns its endpoint from the **`NAT_CONNECT` source address** (the
+client uses a single socket), so the **destination** of frames is that endpoint. The
+**source** of frames, however, must be the server's data port — bind a dedicated
+`data_socket` to `('', data_port)` and `sendto` frames from it.
 
-`ConnectionDataPort = 1511` in `NAT_SERVERINFO` remains required (SDK expects it); it describes the server's data port, not a second client listener in this mode.
+`ConnectionDataPort = 1511` in `NAT_SERVERINFO` is required (the SDK uses it to
+recognize the data channel — i.e. which source port valid frames arrive from).
 
 ### When two client ports may still apply
 
@@ -128,23 +134,27 @@ Do **not** assume `data_port = cmd_port + 1` for any client without capture.
 ### What the server must reply (for `Connect()` + `GetServerDescription()`)
 
 1. **`NAT_SERVERINFO` (1)** on the **command port** to the connect datagram source.
-2. Payload: full packed **`sServerDescription`** ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)), including:
-   - `HostPresent = true`
-   - `szHostApp = "Motive"`
-   - `NatNetVersion = {4, 4, 0, 0}`
-   - `bConnectionInfoValid = true`, `ConnectionDataPort = 1511`, `ConnectionMulticast = false` (unicast)
+2. Payload: packed **`sSender_Server`** (279 B), **not** `sServerDescription`. libNatNet
+   parses the `NAT_SERVERINFO` payload as `sSender_Server`; sending the larger
+   `sServerDescription` makes it misread the version/host. Fields:
+   - `Common.szName = "Motive"` (256-byte field)
+   - `Common.Version = {3, 1, 0, 0}` (Motive app), `Common.NatNetVersion = {4, 4, 0, 0}`
+   - `HighResClockFrequency`, `DataPort = 1511`, `IsMulticast = 0` (unicast)
 
-Pre-built in emulator: [`NatNetServer._build_server_description()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py).
+Pre-built in emulator: [`NatNetServer._build_connect_response_payload()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py).
 
 ### After connect (required for `natnet_ros2` topics)
 
 | SDK call | Server must handle |
 |----------|-------------------|
 | `GetDataDescriptionList()` | `NAT_REQUEST_MODELDEF` → `NAT_MODELDEF` with rigid body name/ID (e.g. `"Drone"`) |
-| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** via server command socket; set `rb.params & 0x01` (tracking valid) |
-| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; reply on same client endpoint |
+| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** from the server **data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag; set `rb.params & 0x01` (tracking valid) |
+| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; **send no reply** |
 
-Frame delivery to the client endpoint still needs a valid `NAT_FRAMEOFDATA` serializer in the emulator; the **single-socket client model** above is confirmed via strace + wire capture.
+Verified end-to-end against the real `libNatNet.so` with a C probe that registers
+`SetFrameReceivedCallback` + `NatNet_SetLogCallback`: with the data-port source,
+EOD tag, `sSender_Server` reply, and no keepalive reply, the probe reports
+`Server: Motive 3.1.0.0 NatNet 4.4.0.0`, `data descriptions: 1`, and ~74 Hz callbacks.
 
 ## Wire format reference (do not confuse)
 
@@ -167,7 +177,7 @@ Not part of the repo. If you need to re-verify wire behavior or debug a new clie
    - `tcpdump -i any udp and host <client_ip>` or the stub's packet log
    - `strace -e trace=bind,sendto,recvfrom` on the client binary
    - `/proc/<pid>/net/udp` or `ss -uapn` (treat **`NAT_CONNECT` source port** as ground truth if they disagree)
-5. **Frame delivery check** — once the emulator can emit valid `NAT_FRAMEOFDATA`, confirm the client's frame callback fires when sending to the `NAT_CONNECT` source endpoint from the server command socket.
+5. **Frame delivery check** — confirm the client's frame callback fires. Register both `SetFrameReceivedCallback` **and** `NatNet_SetLogCallback` (the log callback surfaces silent drops). Frames must be sent from the server **data socket** (source port = `data_port`) and end with the 4-byte EOD tag, or the SDK drops them with no callback.
 
 Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not in-memory `sNatNetClientConnectParams`.
 
@@ -176,7 +186,7 @@ Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not
 1. **Command listener** on `0.0.0.0:1510`
 2. **`NAT_CONNECT`** → register `client_endpoint` from `recvfrom`; reply `NAT_SERVERINFO`
 3. **`NAT_REQUEST_MODELDEF`** → reply `NAT_MODELDEF` (match `body_name` in config)
-4. **Frame loop** → `NAT_FRAMEOFDATA` to **`client_endpoint` via command socket** (libNatNet 4.4 unicast — same port as connect, not `cmd+1`)
+4. **Frame loop** → `NAT_FRAMEOFDATA` to `client_endpoint` **from the data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag
 5. **Isaac integration** → sample drone pose → `sFrameOfMocapData` → `enqueue_mocap_data()`
 6. **Docker** → emulator on `172.31.0.200`; robot `server_ip` points there
 
@@ -186,7 +196,7 @@ Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not
 |-------|----------|-----------|
 | Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names |
 | Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model |
-| Integration (planned) | `tests/sim/motive_emulator/` | Full SDK parser + `natnet_ros2_node` |
+| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (marks: `integration`, `natnet`) |
 | System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` |
 
 ```bash
@@ -198,4 +208,4 @@ docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-sele
 
 - OptiTrack NatNet docs: https://docs.optitrack.com/developer-tools/natnet-sdk/natnet-4.0
 - SDK samples (wire format): `NatNet_SDK_*/Samples/PacketClient/`, `PythonClient/` (legacy connect in Python only)
-- Integration test plan: [`tests/sim/motive_emulator/README.md`](../../../tests/sim/motive_emulator/README.md)
+- Integration test: [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md)
diff --git a/AGENTS.md b/AGENTS.md
index 6bb05ad0a..fa00915a4 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -197,7 +197,7 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo <topic_name> --onc
    - Verify module behavior in isolation
    - Test with synthetic data
    - Located in module's `test/` directory
-   - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m <mark>` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`).
+   - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m <mark>` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`unit`, `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`, `integration`, `natnet`).
 
    ```bash
    docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+"
@@ -205,7 +205,9 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo <topic_name> --onc
 
 2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `<package>/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) re-export those tests so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill.
 
-3. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy)
+3. **Integration (`pytest`, `integration` mark, [`tests/integration/`](tests/integration/)):** Wire a few real components together — the robot autonomy container plus a host-side component — **without** a sim or GPU. The shared `robot_autonomy_stack` fixture reuses a running `robot-desktop` container or brings one up only when `--run-integration` is passed (else skips), so a plain `pytest tests/` never spins up Docker. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz; marks `integration`, `natnet`). Example: `airstack test -m integration --run-integration -v`.
+
+4. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy)
    - End-to-end autonomy stack testing
    - Real sensor simulation
    - Multi-robot scenarios
diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md
index f69056008..2b97c787d 100644
--- a/docs/development/intermediate/testing/unit_testing.md
+++ b/docs/development/intermediate/testing/unit_testing.md
@@ -184,8 +184,8 @@ The `build_packages` CI job (`tests/system/test_build_packages.py`) also runs
 ## Extending to sim and GCS
 
 The proxy pattern extends to other components. As sim-side Python logic (e.g. the
-[Motive emulator](../../../../tests/sim/motive_emulator/README.md)) and GCS modules
-acquire unit-testable code, follow the same layout:
+[NatNet emulator](../../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md))
+and GCS modules acquire unit-testable code, follow the same layout:
 
 ```
 simulation/.../<tool>/test/test_<name>.py   ← source
diff --git a/tests/README.md b/tests/README.md
index f10942ff7..e8c5b1e89 100644
--- a/tests/README.md
+++ b/tests/README.md
@@ -1,10 +1,11 @@
 # Testing (`tests/`)
 
-AirStack's **pytest** tree under `tests/` has three roles:
+AirStack's **pytest** tree under `tests/` has four roles:
 
 1. **`tests/system/`** — Docker stack tests (sim + robot + GCS): liveliness, sensor Hz, takeoff/hover/land, image/workspace builds.
-2. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`.
-3. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. Motive / NatNet emulator). Mark: `unit`.
+2. **`tests/integration/`** — Cross-component tests that need the robot container plus a host-side component, but **no sim/GPU** (e.g. the NatNet emulator). Mark: `integration`.
+3. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`.
+4. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. NatNet emulator). Mark: `unit`.
 
 Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for hermetic tests only, or the marks below for the full stack.
 
@@ -24,6 +25,27 @@ Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for
 | [`system/test_sensors.py`](system/test_sensors.py) | `sensors` | After liveliness in collection order: sim + robot stereo/depth Hz (**Isaac:** batched ``ros2 topic hz`` to avoid bridge overload; **ms-airsim:** single batch), filtered LiDAR via ``echo --once`` + cloud sanity (isaacsim), sim RTF, ``test_sensor_streams_stable`` | Docker daemon, GPU, sim license |
 | [`system/test_takeoff_hover_land.py`](system/test_takeoff_hover_land.py) | `takeoff_hover_land` | End-to-end flight: PX4 readiness gate, takeoff to 10 m, hover stability, land — one chain per (sim, num_robots, iteration, velocity) | Docker daemon, GPU, sim license |
 
+### Integration tests (`tests/integration/`)
+
+The **integration** tier wires a few real components together — typically the
+robot autonomy container plus a host-side component — without a simulator or GPU.
+See [`integration/README.md`](integration/README.md).
+
+| Scenario | Mark(s) | What it tests | Hardware required |
+|----------|---------|---------------|-------------------|
+| [`integration/natnet/`](integration/natnet/) | `integration`, `natnet` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim) |
+
+The shared `robot_autonomy_stack` fixture (in [`conftest.py`](conftest.py))
+reuses a running `robot-desktop` container, or brings one up only when
+`--run-integration` is passed (otherwise the test skips). So a plain
+`pytest tests/` never spins up Docker for this tier.
+
+```bash
+# Reuse a running container, or bring one up on demand:
+pytest tests/integration/ -m integration -v                    # reuse-or-skip
+pytest tests/integration/ -m integration --run-integration -v  # bring up / tear down
+```
+
 ### Unit tests (`tests/robot/`, `tests/sim/`)
 
 Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)).
@@ -217,6 +239,7 @@ pytest tests/ -m sensors \
 | `--stable-interval` | `10` | Seconds between polls in those stability tests |
 | `--gui` | off | Show simulator GUI (disables headless mode) |
 | `--takeoff-velocities` | `0.5,1,2` | Takeoff/land speeds in m/s |
+| `--run-integration` | off | Let `tests/integration/` tests bring up the robot container themselves (else reuse-or-skip) |
 
 ---
 

From 4ce3712ee09698d8512e7c8f1312603c1dd03223 Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Tue, 9 Jun 2026 12:59:39 -0400
Subject: [PATCH 12/27] incremented version tag

---
 .env | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.env b/.env
index 82cc01ccb..0d994bc51 100644
--- a/.env
+++ b/.env
@@ -12,7 +12,7 @@ PROJECT_NAME="airstack"
 # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
 # auto-generated from git commit hash
-VERSION="0.19.0-alpha.3"
+VERSION="0.19.0-alpha.4"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
 DOCKER_IMAGE_BUILD_MODE="dev"  
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.

From c427472c05d702b87aa8c52f2d0a70934af9428c Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Tue, 9 Jun 2026 15:27:31 -0400
Subject: [PATCH 13/27] feat(isaac-sim): install and mount
 optitrack.natnet.emulator extension

NatNet emulator installation is baked into the isaac-sim image, bind-mounted for live edits, and enabled in Kit config.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 simulation/isaac-sim/docker/Dockerfile.isaac-ros      | 4 ++++
 simulation/isaac-sim/docker/docker-compose.yaml       | 5 ++++-
 simulation/isaac-sim/docker/user_TEMPLATE.config.json | 3 ++-
 3 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros
index 0dca11fb7..c563bf2c8 100644
--- a/simulation/isaac-sim/docker/Dockerfile.isaac-ros
+++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros
@@ -145,6 +145,10 @@ ENV ACCEPT_EULA="Y"
 # ENV ISAACSIM_PYTHON=/isaac-sim/python.sh
 RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator
 
+# OptiTrack NatNet emulator extension (in-repo; compose mount overrides at runtime)
+COPY extensions/optitrack.natnet.emulator /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator
+RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator
+
 # Install PX4 things
 RUN git clone --branch ${PX4_VERSION} --recursive https://github.com/PX4/PX4-Autopilot.git
 
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index dfd699aa4..de2ff5ae5 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -68,9 +68,11 @@ services:
       - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw \
       # pegasus integration
       - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw
+      # optitrack natnet emulator
+      - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw
       # omniverse
       - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw
-      - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus extension; IMPORTANT: set the version number without the trailing .0
+      - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus + optitrack extensions; IMPORTANT: set the version number without the trailing .0
       # developer stuff
       - .dev:/isaac-sim/.dev:rw # developer config
       - .bashrc:/isaac-sim/.bashrc:rw # bash config
@@ -143,6 +145,7 @@ services:
       - $HOME/docker/isaac-sim/data:/isaac-sim/.local/share/ov/data:rw
       - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw
       - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw
+      - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw
       - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw
       - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw
       - .dev:/isaac-sim/.dev:rw
diff --git a/simulation/isaac-sim/docker/user_TEMPLATE.config.json b/simulation/isaac-sim/docker/user_TEMPLATE.config.json
index 0ce7c11f0..26b6093c5 100755
--- a/simulation/isaac-sim/docker/user_TEMPLATE.config.json
+++ b/simulation/isaac-sim/docker/user_TEMPLATE.config.json
@@ -544,7 +544,8 @@
             },
             "exts": {
                 "enabled": {
-                    "0": "pegasus.simulator-5.1.0"
+                    "0": "pegasus.simulator-5.1.0",
+                    "1": "optitrack.natnet.emulator-0.1.0"
                 }
             },
             "window": {

From 8c2cfc64384ccad79ea5d815eaa980c1e5f0a36c Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Wed, 10 Jun 2026 11:28:53 -0400
Subject: [PATCH 14/27] NatNet server object stub with isaac-sim extension
 implemented

---
 .../config/extension.toml                     |   8 +
 .../natnet/emulator/isaac/__init__.py         |  32 ++
 .../optitrack/natnet/emulator/isaac/config.py | 198 ++++++++++
 .../natnet/emulator/isaac/ui_extension.py     | 348 ++++++++++++++++++
 .../natnet/emulator/isaac/usd_bindings.py     | 161 ++++++++
 .../schema/schema.usda                        | 100 +++++
 .../test/test_interface_authoring.py          | 102 +++++
 .../test/test_interface_config.py             | 149 ++++++++
 tests/requirements.txt                        |   1 +
 tests/sim/README.md                           |   2 +-
 .../test_interface_authoring.py               |  15 +
 .../test_interface_config.py                  |  11 +
 12 files changed, 1126 insertions(+), 1 deletion(-)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_interface_config.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
index b56f9d079..a5df394da 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml
@@ -7,9 +7,17 @@ keywords = ["optitrack", "natnet", "mocap", "simulation"]
 
 [dependencies]
 "omni.isaac.core" = {}
+"omni.usd" = {}
+"omni.ui" = {}
+"omni.kit.menu.utils" = {}
 
+# Pure transport/types package (no Kit UI; safe to import anywhere).
 [[python.module]]
 name = "optitrack.natnet.emulator"
 
+# Kit UI entry point: NatNetEmulatorExtension (menu + config-prim authoring window).
+[[python.module]]
+name = "optitrack.natnet.emulator.isaac.ui_extension"
+
 [python.build-system]
 requires = ["setuptools"]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
new file mode 100644
index 000000000..62b9882e9
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -0,0 +1,32 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Isaac Sim integration for the NatNet emulator (stage-driven config prim).
+
+``config`` is pure Python (no USD). ``usd_bindings`` imports ``pxr`` lazily, so
+importing this package is safe in non-Isaac environments; the USD functions only
+require ``pxr`` when actually called.
+"""
+
+from .config import (
+    BodyBinding,
+    NatNetInterfaceConfig,
+    body_attr_name,
+    make_instance_key,
+)
+from .usd_bindings import (
+    author_interface,
+    find_interfaces,
+    is_interface,
+    read_interface,
+)
+
+__all__ = [
+    "BodyBinding",
+    "NatNetInterfaceConfig",
+    "author_interface",
+    "body_attr_name",
+    "find_interfaces",
+    "is_interface",
+    "make_instance_key",
+    "read_interface",
+]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
new file mode 100644
index 000000000..2bcfbfad3
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
@@ -0,0 +1,198 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Pure-Python config model for the stage-driven NatNet interface.
+
+No USD / Isaac imports here — this module is hermetically unit-testable. The USD
+binding layer (author/read against a ``Usd.Stage``) lives in ``usd_bindings.py``
+and depends on this model. Keeping the two split is the design's "design for
+testability" rule: nearly all logic stays free of ``pxr``.
+
+Attribute names follow the multi-apply schema convention
+(``natnet:body:<key>:<field>``) so the custom-attribute backing used today is a
+drop-in for a future typed applied schema.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Iterable, Mapping
+
+# --- attribute name constants (USD property names) -----------------------------
+
+ATTR_NAMESPACE = "natnet"
+MARKER_ATTR = "natnet:isInterface"
+
+ATTR_SERVER_ENABLED = "natnet:serverEnabled"
+ATTR_SERVER_IP = "natnet:serverIp"
+ATTR_MODE = "natnet:mode"
+ATTR_MULTICAST_ADDR = "natnet:multicastAddr"
+ATTR_COMMAND_PORT = "natnet:commandPort"
+ATTR_DATA_PORT = "natnet:dataPort"
+ATTR_PUBLISH_RATE = "natnet:publishRate"
+ATTR_NATNET_VERSION = "natnet:natnetVersion"
+
+BODY_PREFIX = "natnet:body:"
+BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName"
+BODY_FIELD_STREAMING_ID = "streamingId"
+BODY_FIELD_PARENT_ID = "parentId"
+BODY_FIELD_TARGET = "target"
+
+VALID_MODES = ("unicast", "multicast")
+
+# defaults shared with NatNetUnicastServer
+DEFAULT_SERVER_IP = "172.31.0.200"
+DEFAULT_MULTICAST_ADDR = "239.255.42.99"
+DEFAULT_COMMAND_PORT = 1510
+DEFAULT_DATA_PORT = 1511
+DEFAULT_PUBLISH_RATE = 100.0
+DEFAULT_NATNET_VERSION = "4.4.0.0"
+
+
+def body_attr_name(key: str, field_name: str) -> str:
+    """USD property name for a body-binding field on the given instance key."""
+    return f"{BODY_PREFIX}{key}:{field_name}"
+
+
+def make_instance_key(name: str, used: set[str]) -> str:
+    """Derive a valid, unique multi-apply instance token from a rigid body name.
+
+    USD property/instance tokens must be identifier-like; sanitize non-alnum chars
+    to underscores and disambiguate collisions with a numeric suffix.
+    """
+    sanitized = "".join(c if c.isalnum() else "_" for c in name).strip("_")
+    if not sanitized:
+        sanitized = "body"
+    if sanitized[0].isdigit():
+        sanitized = f"b_{sanitized}"
+    key = sanitized
+    i = 1
+    while key in used:
+        key = f"{sanitized}_{i}"
+        i += 1
+    used.add(key)
+    return key
+
+
+@dataclass
+class BodyBinding:
+    """One tracked rigid body: a Motive name/ID mapped to a USD prim path."""
+
+    rigid_body_name: str
+    target_prim: str
+    streaming_id: int = 1
+    parent_id: int = -1
+
+    @classmethod
+    def from_dict(cls, data: Mapping[str, Any], *, target_prim: str | None = None) -> "BodyBinding":
+        d = dict(data)
+        resolved_target = target_prim if target_prim is not None else d.get("target_prim")
+        if not resolved_target:
+            raise ValueError("BodyBinding requires a target_prim (USD path of the tracked prim)")
+        if "rigid_body_name" not in d:
+            raise ValueError("BodyBinding requires a rigid_body_name")
+        return cls(
+            rigid_body_name=str(d["rigid_body_name"]),
+            target_prim=str(resolved_target),
+            streaming_id=int(d.get("streaming_id", 1)),
+            parent_id=int(d.get("parent_id", -1)),
+        )
+
+    def to_dict(self) -> dict[str, Any]:
+        return {
+            "rigid_body_name": self.rigid_body_name,
+            "target_prim": self.target_prim,
+            "streaming_id": self.streaming_id,
+            "parent_id": self.parent_id,
+        }
+
+
+def _normalize_bodies(bodies: Any) -> list[BodyBinding]:
+    """Accept a list of dicts/BodyBindings, or a ``{prim_path: {...}}`` mapping."""
+    if bodies is None:
+        return []
+    out: list[BodyBinding] = []
+    if isinstance(bodies, Mapping):
+        for prim_path, body in bodies.items():
+            out.append(BodyBinding.from_dict(body, target_prim=prim_path))
+        return out
+    if isinstance(bodies, Iterable):
+        for body in bodies:
+            if isinstance(body, BodyBinding):
+                out.append(body)
+            else:
+                out.append(BodyBinding.from_dict(body))
+        return out
+    raise ValueError(f"`bodies` must be a list or a mapping, got {type(bodies).__name__}")
+
+
+@dataclass
+class NatNetInterfaceConfig:
+    """Server-level config plus the body catalog for one NatNet interface prim."""
+
+    server_enabled: bool = True
+    server_ip: str = DEFAULT_SERVER_IP
+    mode: str = "unicast"
+    multicast_addr: str = DEFAULT_MULTICAST_ADDR
+    command_port: int = DEFAULT_COMMAND_PORT
+    data_port: int = DEFAULT_DATA_PORT
+    publish_rate: float = DEFAULT_PUBLISH_RATE
+    natnet_version: str = DEFAULT_NATNET_VERSION
+    bodies: list[BodyBinding] = field(default_factory=list)
+
+    @classmethod
+    def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig":
+        d = dict(data)
+        return cls(
+            server_enabled=bool(d.get("server_enabled", True)),
+            server_ip=str(d.get("server_ip", DEFAULT_SERVER_IP)),
+            mode=str(d.get("mode", "unicast")),
+            multicast_addr=str(d.get("multicast_addr", DEFAULT_MULTICAST_ADDR)),
+            command_port=int(d.get("command_port", DEFAULT_COMMAND_PORT)),
+            data_port=int(d.get("data_port", DEFAULT_DATA_PORT)),
+            publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)),
+            natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)),
+            bodies=_normalize_bodies(d.get("bodies")),
+        )
+
+    def to_dict(self) -> dict[str, Any]:
+        return {
+            "server_enabled": self.server_enabled,
+            "server_ip": self.server_ip,
+            "mode": self.mode,
+            "multicast_addr": self.multicast_addr,
+            "command_port": self.command_port,
+            "data_port": self.data_port,
+            "publish_rate": self.publish_rate,
+            "natnet_version": self.natnet_version,
+            "bodies": [b.to_dict() for b in self.bodies],
+        }
+
+    def validate(self) -> "NatNetInterfaceConfig":
+        """Raise ``ValueError`` (aggregating all problems) if the config is invalid."""
+        errors: list[str] = []
+        if self.mode not in VALID_MODES:
+            errors.append(f"mode must be one of {VALID_MODES}, got {self.mode!r}")
+        for port_name, port in (("command_port", self.command_port), ("data_port", self.data_port)):
+            if not (0 < port < 65536):
+                errors.append(f"{port_name} must be in 1..65535, got {port}")
+        if self.command_port == self.data_port:
+            errors.append("command_port and data_port must differ")
+        if self.publish_rate <= 0:
+            errors.append(f"publish_rate must be > 0, got {self.publish_rate}")
+        for i, body in enumerate(self.bodies):
+            if not body.rigid_body_name:
+                errors.append(f"body[{i}] rigid_body_name must be non-empty")
+        names = [b.rigid_body_name for b in self.bodies]
+        if len(set(names)) != len(names):
+            errors.append("rigid_body_name values must be unique across bodies")
+        ids = [b.streaming_id for b in self.bodies]
+        if len(set(ids)) != len(ids):
+            errors.append("streaming_id values must be unique across bodies")
+        if errors:
+            raise ValueError("Invalid NatNetInterfaceConfig: " + "; ".join(errors))
+        return self
+
+    def assign_instance_keys(self) -> list[tuple[str, BodyBinding]]:
+        """Pair each body with a deterministic, unique multi-apply instance key."""
+        used: set[str] = set()
+        return [(make_instance_key(b.rigid_body_name, used), b) for b in self.bodies]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
new file mode 100644
index 000000000..df0496736
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -0,0 +1,348 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Kit extension entry: docked editor for the NatNet interface config prim.
+
+Commit 1 scope — create/manage ``/World/NatNetInterface``. The window docks to the
+bottom-right (alongside the Property panel, like Pegasus) so it's easy to find.
+
+Sync model is **explicit and user-driven** via the top button row:
+
+* **Load from Stage** — read the prim into the window (or reset to defaults if none).
+* **Save** — write the window's current values to the prim (validate + author).
+* **Create Server** — ensure the interface prim exists (authored from the current
+  values) and select it. (Actually starting the UDP server arrives in a later commit;
+  for now this creates/initializes the interface prim that the server will read.)
+
+Edits in the window mutate an in-memory working copy only; nothing touches the prim
+until you Save / Create, so you are in charge of keeping things synced.
+
+All ``omni.*`` imports live inside methods so this module never imports Kit at load
+time outside a running Kit process. Authoring goes through the tested, USD-only
+:mod:`optitrack.natnet.emulator.isaac.usd_bindings` facade.
+"""
+
+from __future__ import annotations
+
+import omni.ext
+
+from .config import VALID_MODES, BodyBinding, NatNetInterfaceConfig
+from .usd_bindings import author_interface, find_interfaces, read_interface
+
+_DEFAULT_PRIM_PATH = "/World/NatNetInterface"
+_LABEL_WIDTH = 140
+
+
+class NatNetEmulatorExtension(omni.ext.IExt):
+    """Registers the Window menu entry + the docked editor panel."""
+
+    def on_startup(self, ext_id):  # noqa: D401 - Kit lifecycle hook
+        self._window = None
+        self._bodies_frame = None
+        self._cfg = NatNetInterfaceConfig()
+        self._add_menu()
+
+    def on_shutdown(self):
+        self._remove_menu()
+        if self._window is not None:
+            self._window.destroy()
+            self._window = None
+
+    # --- menu ------------------------------------------------------------------
+
+    def _add_menu(self):
+        try:
+            import omni.kit.menu.utils as menu_utils
+            from omni.kit.menu.utils import MenuItemDescription
+        except Exception:  # pragma: no cover - Kit only
+            return
+        self._menu_entries = [
+            MenuItemDescription(name="NatNet Interface", onclick_fn=self._toggle_window)
+        ]
+        menu_utils.add_menu_items(self._menu_entries, "Window")
+
+    def _remove_menu(self):
+        try:
+            import omni.kit.menu.utils as menu_utils
+        except Exception:  # pragma: no cover - Kit only
+            return
+        if getattr(self, "_menu_entries", None):
+            menu_utils.remove_menu_items(self._menu_entries, "Window")
+            self._menu_entries = None
+
+    # --- window ----------------------------------------------------------------
+
+    def _toggle_window(self, *_):
+        import omni.ui as ui
+
+        if self._window is None:
+            self._window = ui.Window("NatNet Interface", width=400, height=600)
+            self._window.frame.set_build_fn(self._build_window)
+            # Dock bottom-right next to the Property panel, like Pegasus.
+            self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE)
+            self._window.visible = True
+            return
+        self._window.visible = not self._window.visible
+
+    def _refresh(self, *_):
+        if self._window is not None:
+            self._window.frame.rebuild()
+
+    def _build_window(self):
+        import omni.ui as ui
+
+        with ui.ScrollingFrame():
+            with ui.VStack(spacing=6, height=0):
+                ui.Label("NatNet interface", height=0, style={"font_size": 16})
+
+                with ui.HStack(height=28, spacing=6):
+                    ui.Button("Create Server", clicked_fn=self._create_server)
+                    ui.Button("Save", clicked_fn=self._save)
+                    ui.Button("Load from Stage", clicked_fn=self._load_from_stage)
+
+                ui.Label(
+                    "\u26a0  Remember to save after each edit",
+                    height=0,
+                    word_wrap=True,
+                    style={"color": 0xFF33CCFF, "font_size": 14},
+                )
+
+                ui.Label(self._status_text(), height=0, word_wrap=True)
+
+                ui.Separator(height=6)
+                self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled)
+                self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip)
+                self._combo_row(ui, "Mode", "mode", self._cfg.mode)
+                self._int_row(ui, "Command port", "command_port", self._cfg.command_port)
+                self._int_row(ui, "Data port", "data_port", self._cfg.data_port)
+                self._float_row(ui, "Publish rate (Hz)", "publish_rate", self._cfg.publish_rate)
+
+                ui.Separator(height=6)
+                ui.Label("Tracked bodies", height=0, style={"font_size": 14})
+                self._bodies_frame = ui.Frame(height=0)
+                self._bodies_frame.set_build_fn(self._build_bodies)
+                with ui.HStack(height=0, spacing=6):
+                    ui.Button("Add body (from selection)", clicked_fn=self._add_body)
+
+    def _status_text(self):
+        prim = self._find_interface()
+        if prim is None:
+            return "No prim on stage yet — Save or Create Server to author one."
+        return f"Prim on stage: {prim.GetPath().pathString} (Save to push edits, Load to pull)"
+
+    # --- server field rows (edit the working copy only) ------------------------
+
+    def _bool_row(self, ui, label, key, value):
+        with ui.HStack(height=0):
+            ui.Label(label, width=_LABEL_WIDTH)
+            cb = ui.CheckBox()
+            cb.model.set_value(bool(value))
+            cb.model.add_value_changed_fn(
+                lambda m, k=key: self._set_cfg_field(k, m.get_value_as_bool())
+            )
+
+    def _str_row(self, ui, label, key, value):
+        with ui.HStack(height=0):
+            ui.Label(label, width=_LABEL_WIDTH)
+            model = ui.StringField().model
+            model.set_value(str(value))
+            model.add_value_changed_fn(
+                lambda m, k=key: self._set_cfg_field(k, m.get_value_as_string())
+            )
+
+    def _int_row(self, ui, label, key, value):
+        with ui.HStack(height=0):
+            ui.Label(label, width=_LABEL_WIDTH)
+            model = ui.IntField().model
+            model.set_value(int(value))
+            model.add_value_changed_fn(
+                lambda m, k=key: self._set_cfg_field(k, m.get_value_as_int())
+            )
+
+    def _float_row(self, ui, label, key, value):
+        with ui.HStack(height=0):
+            ui.Label(label, width=_LABEL_WIDTH)
+            model = ui.FloatField().model
+            model.set_value(float(value))
+            model.add_value_changed_fn(
+                lambda m, k=key: self._set_cfg_field(k, m.get_value_as_float())
+            )
+
+    def _combo_row(self, ui, label, key, value):
+        with ui.HStack(height=0):
+            ui.Label(label, width=_LABEL_WIDTH)
+            index = VALID_MODES.index(value) if value in VALID_MODES else 0
+            combo = ui.ComboBox(index, *VALID_MODES)
+            combo.model.get_item_value_model().add_value_changed_fn(
+                lambda m: self._set_cfg_field("mode", VALID_MODES[m.get_value_as_int()])
+            )
+
+    def _set_cfg_field(self, attr, value):
+        setattr(self._cfg, attr, value)
+
+    # --- bodies ----------------------------------------------------------------
+
+    def _rebuild_bodies(self, *_):
+        if self._bodies_frame is not None:
+            self._bodies_frame.rebuild()
+
+    def _build_bodies(self):
+        import omni.ui as ui
+
+        with ui.VStack(spacing=4, height=0):
+            if not self._cfg.bodies:
+                ui.Label("  (no bodies — select a prim and click Add body)", height=0)
+                return
+            with ui.HStack(height=0, spacing=4):
+                ui.Label("Rigid body name", width=ui.Fraction(1))
+                ui.Label("ID", width=40)
+                ui.Label("Parent", width=50)
+                ui.Label("Target prim", width=ui.Fraction(2))
+                ui.Spacer(width=98)
+            for idx, body in enumerate(self._cfg.bodies):
+                self._build_body_row(ui, idx, body)
+
+    def _build_body_row(self, ui, idx, body):
+        with ui.HStack(height=0, spacing=4):
+            name = ui.StringField(width=ui.Fraction(1)).model
+            name.set_value(body.rigid_body_name)
+            name.add_value_changed_fn(
+                lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string())
+            )
+
+            sid = ui.IntField(width=40).model
+            sid.set_value(body.streaming_id)
+            sid.add_value_changed_fn(
+                lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int())
+            )
+
+            parent = ui.IntField(width=50).model
+            parent.set_value(body.parent_id)
+            parent.add_value_changed_fn(
+                lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int())
+            )
+
+            target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model
+            target.set_value(body.target_prim)
+            target.add_value_changed_fn(
+                lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string())
+            )
+
+            ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i))
+            ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i))
+
+    def _set_body_field(self, index, attr, value):
+        if 0 <= index < len(self._cfg.bodies):
+            setattr(self._cfg.bodies[index], attr, value)
+
+    def _add_body(self):
+        next_id = max((b.streaming_id for b in self._cfg.bodies), default=0) + 1
+        target = self._selected_target_path(self._find_interface())
+        name = target.rsplit("/", 1)[-1] if target else f"Body{next_id}"
+        existing = {b.rigid_body_name for b in self._cfg.bodies}
+        while name in existing:
+            name = f"{name}_{next_id}"
+        self._cfg.bodies.append(BodyBinding(rigid_body_name=name, target_prim=target, streaming_id=next_id))
+        self._rebuild_bodies()
+
+    def _remove_body_at(self, index):
+        if 0 <= index < len(self._cfg.bodies):
+            self._cfg.bodies.pop(index)
+            self._rebuild_bodies()
+
+    def _retarget_body(self, index):
+        import carb
+
+        path = self._selected_target_path(self._find_interface())
+        if not path:
+            carb.log_warn("[natnet] Select a prim in the viewport to retarget this body.")
+            return
+        if 0 <= index < len(self._cfg.bodies):
+            self._cfg.bodies[index].target_prim = path
+            self._rebuild_bodies()
+
+    # --- stage helpers ---------------------------------------------------------
+
+    def _get_stage(self):
+        import omni.usd
+
+        return omni.usd.get_context().get_stage()
+
+    def _find_interface(self):
+        stage = self._get_stage()
+        if stage is None:
+            return None
+        interfaces = find_interfaces(stage)
+        return interfaces[0] if interfaces else None
+
+    def _interface_path(self):
+        prim = self._find_interface()
+        return prim.GetPath().pathString if prim is not None else _DEFAULT_PRIM_PATH
+
+    def _select(self, prim_path):
+        import omni.usd
+
+        omni.usd.get_context().get_selection().set_selected_prim_paths([prim_path], True)
+
+    def _selected_target_path(self, interface_prim):
+        import omni.usd
+
+        sel = omni.usd.get_context().get_selection().get_selected_prim_paths()
+        iface_path = interface_prim.GetPath().pathString if interface_prim else None
+        for path in sel:
+            if path != iface_path:
+                return path
+        return ""
+
+    # --- explicit sync actions -------------------------------------------------
+
+    def _save(self):
+        import carb
+
+        stage = self._get_stage()
+        if stage is None:
+            carb.log_error("[natnet] No active stage.")
+            return
+        try:
+            self._cfg.validate()
+        except ValueError as exc:
+            carb.log_error(f"[natnet] Not saved: {exc}")
+            return
+        path = self._interface_path()
+        author_interface(stage, path, self._cfg)
+        carb.log_info(f"[natnet] Saved interface to {path} ({len(self._cfg.bodies)} bodies).")
+        self._refresh()
+
+    def _load_from_stage(self):
+        import carb
+
+        prim = self._find_interface()
+        if prim is None:
+            self._cfg = NatNetInterfaceConfig()
+            carb.log_warn("[natnet] No interface on stage — reset to defaults.")
+        else:
+            self._cfg = read_interface(prim)
+            carb.log_info(f"[natnet] Loaded interface from {prim.GetPath().pathString}.")
+        self._refresh()
+
+    def _create_server(self):
+        import carb
+
+        stage = self._get_stage()
+        if stage is None:
+            carb.log_error("[natnet] No active stage.")
+            return
+        prim = self._find_interface()
+        if prim is None:
+            try:
+                self._cfg.validate()
+            except ValueError as exc:
+                carb.log_error(f"[natnet] Cannot create: {exc}")
+                return
+            author_interface(stage, _DEFAULT_PRIM_PATH, self._cfg)
+            path = _DEFAULT_PRIM_PATH
+            carb.log_info(f"[natnet] Created interface prim at {path}. (Server start: later commit.)")
+        else:
+            path = prim.GetPath().pathString
+            carb.log_info(f"[natnet] Interface already exists at {path}. (Server start: later commit.)")
+        self._select(path)
+        self._refresh()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
new file mode 100644
index 000000000..a355878bb
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -0,0 +1,161 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""USD binding layer: author / read / find NatNet interface prims on a stage.
+
+``pxr`` is imported **lazily inside each function** so importing this module never
+requires USD — that keeps ``optitrack.natnet.emulator.isaac`` importable in plain
+(non-Isaac) test environments. The pure config logic lives in ``config.py``.
+
+Backing today is **plain namespaced custom attributes + relationships** (the
+registration-free fallback from the design's Risk #1). Property names follow the
+multi-apply schema convention (``natnet:body:<key>:<field>``), so swapping to a
+typed codeless applied schema later requires no change to readers/writers.
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+from .config import (
+    ATTR_COMMAND_PORT,
+    ATTR_DATA_PORT,
+    ATTR_MODE,
+    ATTR_MULTICAST_ADDR,
+    ATTR_NATNET_VERSION,
+    ATTR_PUBLISH_RATE,
+    ATTR_SERVER_ENABLED,
+    ATTR_SERVER_IP,
+    BODY_FIELD_PARENT_ID,
+    BODY_FIELD_RIGID_BODY_NAME,
+    BODY_FIELD_STREAMING_ID,
+    BODY_FIELD_TARGET,
+    BODY_PREFIX,
+    DEFAULT_COMMAND_PORT,
+    DEFAULT_DATA_PORT,
+    DEFAULT_MULTICAST_ADDR,
+    DEFAULT_NATNET_VERSION,
+    DEFAULT_PUBLISH_RATE,
+    DEFAULT_SERVER_IP,
+    MARKER_ATTR,
+    BodyBinding,
+    NatNetInterfaceConfig,
+    body_attr_name,
+)
+
+
+def author_interface(stage, prim_path: str, config: Any) -> Any:
+    """Create/overwrite a NatNet interface prim at ``prim_path`` from ``config``.
+
+    ``config`` may be a :class:`NatNetInterfaceConfig` or a plain ``dict`` (passed
+    through ``from_dict``). Returns the ``Usd.Prim``.
+    """
+    from pxr import Sdf
+
+    cfg = config if isinstance(config, NatNetInterfaceConfig) else NatNetInterfaceConfig.from_dict(config)
+    cfg.validate()
+
+    prim = stage.DefinePrim(prim_path, "Scope")
+
+    # Overwrite semantics: drop any previously-authored body properties so removed
+    # bodies don't linger across re-authoring.
+    _clear_body_properties(prim)
+
+    _set(prim, MARKER_ATTR, Sdf.ValueTypeNames.Bool, True)
+    _set(prim, ATTR_SERVER_ENABLED, Sdf.ValueTypeNames.Bool, cfg.server_enabled)
+    _set(prim, ATTR_SERVER_IP, Sdf.ValueTypeNames.String, cfg.server_ip)
+    _set(prim, ATTR_MODE, Sdf.ValueTypeNames.Token, cfg.mode)
+    _set(prim, ATTR_MULTICAST_ADDR, Sdf.ValueTypeNames.String, cfg.multicast_addr)
+    _set(prim, ATTR_COMMAND_PORT, Sdf.ValueTypeNames.Int, cfg.command_port)
+    _set(prim, ATTR_DATA_PORT, Sdf.ValueTypeNames.Int, cfg.data_port)
+    _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate)
+    _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version)
+
+    for key, body in cfg.assign_instance_keys():
+        _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name)
+        _set(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), Sdf.ValueTypeNames.Int, body.streaming_id)
+        _set(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), Sdf.ValueTypeNames.Int, body.parent_id)
+        rel = prim.CreateRelationship(body_attr_name(key, BODY_FIELD_TARGET), False)
+        # Empty target is allowed (e.g. a freshly added body to be pointed in the
+        # Property panel); leave the relationship target-less rather than authoring
+        # an invalid empty Sdf.Path.
+        rel.SetTargets([Sdf.Path(body.target_prim)] if body.target_prim else [])
+
+    return prim
+
+
+def read_interface(prim) -> NatNetInterfaceConfig:
+    """Reconstruct a :class:`NatNetInterfaceConfig` from an authored interface prim."""
+    return NatNetInterfaceConfig(
+        server_enabled=bool(_get(prim, ATTR_SERVER_ENABLED, True)),
+        server_ip=str(_get(prim, ATTR_SERVER_IP, DEFAULT_SERVER_IP)),
+        mode=str(_get(prim, ATTR_MODE, "unicast")),
+        multicast_addr=str(_get(prim, ATTR_MULTICAST_ADDR, DEFAULT_MULTICAST_ADDR)),
+        command_port=int(_get(prim, ATTR_COMMAND_PORT, DEFAULT_COMMAND_PORT)),
+        data_port=int(_get(prim, ATTR_DATA_PORT, DEFAULT_DATA_PORT)),
+        publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)),
+        natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)),
+        bodies=_read_bodies(prim),
+    )
+
+
+def find_interfaces(stage) -> list:
+    """Return every prim on the stage marked as a NatNet interface."""
+    interfaces = []
+    for prim in stage.Traverse():
+        attr = prim.GetAttribute(MARKER_ATTR)
+        if attr and attr.HasAuthoredValue() and bool(attr.Get()):
+            interfaces.append(prim)
+    return interfaces
+
+
+def is_interface(prim) -> bool:
+    attr = prim.GetAttribute(MARKER_ATTR)
+    return bool(attr and attr.HasAuthoredValue() and bool(attr.Get()))
+
+
+# --- internal helpers ----------------------------------------------------------
+
+
+def _set(prim, name, type_name, value):
+    attr = prim.CreateAttribute(name, type_name)
+    attr.Set(value)
+    return attr
+
+
+def _get(prim, name, default):
+    attr = prim.GetAttribute(name)
+    if attr and attr.HasAuthoredValue():
+        return attr.Get()
+    return default
+
+
+def _clear_body_properties(prim) -> None:
+    for name in list(prim.GetPropertyNames()):
+        if name.startswith(BODY_PREFIX):
+            prim.RemoveProperty(name)
+
+
+def _read_bodies(prim) -> list[BodyBinding]:
+    suffix = f":{BODY_FIELD_RIGID_BODY_NAME}"
+    keys = [
+        name[len(BODY_PREFIX): -len(suffix)]
+        for name in prim.GetPropertyNames()
+        if name.startswith(BODY_PREFIX) and name.endswith(suffix)
+    ]
+
+    bodies: list[BodyBinding] = []
+    for key in keys:
+        rel = prim.GetRelationship(body_attr_name(key, BODY_FIELD_TARGET))
+        targets = rel.GetTargets() if rel else []
+        bodies.append(
+            BodyBinding(
+                rigid_body_name=str(_get(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), "")),
+                target_prim=str(targets[0]) if targets else "",
+                streaming_id=int(_get(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), 1)),
+                parent_id=int(_get(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), -1)),
+            )
+        )
+
+    # Stable, deterministic order (independent of USD property iteration order).
+    bodies.sort(key=lambda b: (b.streaming_id, b.rigid_body_name))
+    return bodies
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda
new file mode 100644
index 000000000..5f922469e
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda
@@ -0,0 +1,100 @@
+#usda 1.0
+(
+    """
+    NatNet emulator applied API schemas (CODELESS).
+
+    Risk #1 spike (see docs/natnet_interface_prim_design.md): these schemas give the
+    interface prim typed, Property-panel-friendly attributes WITHOUT compiled
+    classes. They are codeless — `skipCodeGeneration = true` below — but still need
+    USD's plugin system to discover the generated registry.
+
+    To produce the registry files (run once, in an env with USD tooling):
+
+        usdGenSchema schema/schema.usda schema/
+
+    That emits `schema/generatedSchema.usda` and `schema/plugInfo.json`. The Kit
+    extension then registers the plugin dir on startup (Plug.Registry().RegisterPlugins).
+
+    Until that registration is verified inside Kit, `optitrack.natnet.emulator.isaac`
+    authors the SAME attribute names as plain namespaced custom attributes (the
+    registration-free fallback), so nothing here is required for the facade to work.
+    """
+    subLayers = [
+        @usd/schema.usda@,
+        @usdGeom/schema.usda@
+    ]
+)
+{
+}
+
+over "GLOBAL" (
+    customData = {
+        bool skipCodeGeneration = true
+        string libraryName = "optitrackNatNet"
+        string libraryPath = "."
+        string libraryPrefix = "OptiTrackNatNet"
+    }
+)
+{
+}
+
+class "NatNetInterfaceAPI" (
+    inherits = </APISchemaBase>
+    customData = {
+        token apiSchemaType = "singleApply"
+    }
+    doc = "Marks a prim as a NatNet emulator interface and holds server-level config."
+)
+{
+    bool natnet:isInterface = true (
+        doc = "Discovery marker — find_interfaces() scans for prims with this set true."
+    )
+    bool natnet:serverEnabled = true (
+        doc = "When true the manager keeps a server running; toggling restarts it."
+    )
+    string natnet:serverIp = "172.31.0.200" (
+        doc = "Server interface IP (NatNetUnicastServer.local_interface)."
+    )
+    token natnet:mode = "unicast" (
+        allowedTokens = ["unicast", "multicast"]
+        doc = "Transmission mode."
+    )
+    string natnet:multicastAddr = "239.255.42.99" (
+        doc = "Multicast group (only used when mode = multicast)."
+    )
+    int natnet:commandPort = 1510 (
+        doc = "NatNet command port."
+    )
+    int natnet:dataPort = 1511 (
+        doc = "NatNet data port (frames stream from this source port)."
+    )
+    float natnet:publishRate = 100 (
+        doc = "Frame publish rate in Hz."
+    )
+    string natnet:natnetVersion = "4.4.0.0" (
+        doc = "Advertised NatNet protocol version."
+    )
+}
+
+class "NatNetBodyBindingAPI" (
+    inherits = </APISchemaBase>
+    customData = {
+        token apiSchemaType = "multipleApply"
+        token propertyNamespacePrefix = "natnet:body"
+    }
+    doc = "One tracked rigid body entry on a NatNet interface prim (apply once per body)."
+)
+{
+    string natnet:body:__INSTANCE_NAME__:rigidBodyName = "" (
+        doc = "Motive rigid body name (sRigidBodyDescription.szName)."
+    )
+    int natnet:body:__INSTANCE_NAME__:streamingId = 1 (
+        doc = "Streaming ID (sRigidBodyDescription.ID)."
+    )
+    int natnet:body:__INSTANCE_NAME__:parentId = -1 (
+        doc = "Parent rigid body ID (-1 if none)."
+    )
+    rel natnet:body:__INSTANCE_NAME__:target (
+        doc = "Tracked prim whose world pose is streamed for this body."
+    )
+}
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
new file mode 100644
index 000000000..0807b70cd
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
@@ -0,0 +1,102 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""USD authoring/read round-trip tests for the NatNet interface prim.
+
+Guarded by ``pytest.importorskip("pxr")`` — these run wherever USD Python is
+available (the Isaac container, or CI when ``usd-core`` is installed from
+``tests/requirements.txt``) and skip cleanly otherwise.
+"""
+
+from __future__ import annotations
+
+import pytest
+
+pytest.importorskip("pxr")
+
+from pxr import Usd  # noqa: E402
+
+from optitrack.natnet.emulator.isaac import (  # noqa: E402
+    author_interface,
+    find_interfaces,
+    read_interface,
+)
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig  # noqa: E402
+
+pytestmark = pytest.mark.unit
+
+_CONFIG = {
+    "server_enabled": True,
+    "server_ip": "172.31.0.200",
+    "mode": "unicast",
+    "command_port": 1510,
+    "data_port": 1511,
+    "publish_rate": 100,
+    "bodies": {
+        "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1},
+        "/World/target": {"rigid_body_name": "Target", "streaming_id": 2, "parent_id": 1},
+    },
+}
+
+
+def _new_stage():
+    return Usd.Stage.CreateInMemory()
+
+
+def test_author_then_find_locates_the_interface():
+    stage = _new_stage()
+    author_interface(stage, "/World/NatNetInterface", _CONFIG)
+    interfaces = find_interfaces(stage)
+    assert [p.GetPath().pathString for p in interfaces] == ["/World/NatNetInterface"]
+
+
+def test_author_read_round_trip():
+    stage = _new_stage()
+    author_interface(stage, "/World/NatNetInterface", _CONFIG)
+    prim = find_interfaces(stage)[0]
+
+    cfg = read_interface(prim)
+    assert cfg.server_ip == "172.31.0.200"
+    assert cfg.mode == "unicast"
+    assert cfg.command_port == 1510
+    assert cfg.data_port == 1511
+
+    by_name = {b.rigid_body_name: b for b in cfg.bodies}
+    assert by_name["Drone"].target_prim == "/World/base_link"
+    assert by_name["Drone"].streaming_id == 1
+    assert by_name["Target"].target_prim == "/World/target"
+    assert by_name["Target"].parent_id == 1
+
+    # read -> author -> read is stable
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    assert read_interface(find_interfaces(stage)[0]) == cfg
+
+
+def test_reauthoring_removes_stale_bodies():
+    stage = _new_stage()
+    author_interface(stage, "/World/NatNetInterface", _CONFIG)
+
+    single = NatNetInterfaceConfig.from_dict(
+        {"bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}]}
+    )
+    author_interface(stage, "/World/NatNetInterface", single)
+
+    cfg = read_interface(find_interfaces(stage)[0])
+    assert [b.rigid_body_name for b in cfg.bodies] == ["Drone"]
+
+
+def test_empty_target_round_trips():
+    # The UI's "Add body" can create a body with no target yet (set later in the
+    # Property panel); it must author and read back cleanly with an empty target.
+    stage = _new_stage()
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)])
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    read = read_interface(find_interfaces(stage)[0])
+    assert read.bodies[0].rigid_body_name == "Drone"
+    assert read.bodies[0].target_prim == ""
+
+
+def test_invalid_config_raises_before_authoring():
+    stage = _new_stage()
+    with pytest.raises(ValueError):
+        author_interface(stage, "/World/NatNetInterface", {"mode": "bogus"})
+    assert find_interfaces(stage) == []
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
new file mode 100644
index 000000000..c015c1a4e
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
@@ -0,0 +1,149 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Hermetic unit tests for the pure-Python NatNet interface config model.
+
+No USD / Isaac imports — exercises dataclasses, dict normalization, the attribute
+name builder, instance-key generation, and validation.
+"""
+
+from __future__ import annotations
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.config import (
+    BodyBinding,
+    NatNetInterfaceConfig,
+    body_attr_name,
+    make_instance_key,
+)
+
+pytestmark = pytest.mark.unit
+
+
+def test_defaults_match_server_expectations():
+    cfg = NatNetInterfaceConfig()
+    assert cfg.server_enabled is True
+    assert cfg.server_ip == "172.31.0.200"
+    assert cfg.mode == "unicast"
+    assert cfg.command_port == 1510
+    assert cfg.data_port == 1511
+    assert cfg.bodies == []
+
+
+def test_from_dict_with_bodies_as_list():
+    cfg = NatNetInterfaceConfig.from_dict(
+        {
+            "server_ip": "10.0.0.5",
+            "bodies": [
+                {"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1},
+            ],
+        }
+    )
+    assert cfg.server_ip == "10.0.0.5"
+    assert len(cfg.bodies) == 1
+    assert cfg.bodies[0] == BodyBinding("Drone", "/World/base_link", 1, -1)
+
+
+def test_from_dict_with_bodies_as_prim_mapping():
+    # The "dictionary of prims -> rigid body names and stuff" form.
+    cfg = NatNetInterfaceConfig.from_dict(
+        {
+            "bodies": {
+                "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1},
+                "/World/target": {"rigid_body_name": "Target", "streaming_id": 2},
+            }
+        }
+    )
+    by_name = {b.rigid_body_name: b for b in cfg.bodies}
+    assert by_name["Drone"].target_prim == "/World/base_link"
+    assert by_name["Target"].target_prim == "/World/target"
+    assert by_name["Target"].streaming_id == 2
+
+
+def test_to_dict_round_trip():
+    cfg = NatNetInterfaceConfig.from_dict(
+        {
+            "mode": "multicast",
+            "publish_rate": 120,
+            "bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link"}],
+        }
+    )
+    restored = NatNetInterfaceConfig.from_dict(cfg.to_dict())
+    assert restored == cfg
+
+
+def test_body_from_dict_requires_target_and_name():
+    with pytest.raises(ValueError):
+        BodyBinding.from_dict({"rigid_body_name": "Drone"})  # no target_prim
+    with pytest.raises(ValueError):
+        BodyBinding.from_dict({"target_prim": "/World/base_link"})  # no name
+
+
+def test_body_attr_name_builder():
+    assert body_attr_name("Drone", "streamingId") == "natnet:body:Drone:streamingId"
+
+
+def test_make_instance_key_sanitizes_and_dedupes():
+    used: set[str] = set()
+    assert make_instance_key("Drone 1", used) == "Drone_1"
+    # collision after sanitization -> numeric suffix
+    assert make_instance_key("Drone-1", used) == "Drone_1_1"
+    # leading digit gets a safe prefix
+    assert make_instance_key("3PO", used).startswith("b_")
+
+
+def test_assign_instance_keys_are_unique():
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("Drone", "/World/a", 1),
+            BodyBinding("Drone", "/World/b", 2),  # duplicate display name
+        ]
+    )
+    keys = [k for k, _ in cfg.assign_instance_keys()]
+    assert len(set(keys)) == 2
+
+
+@pytest.mark.parametrize(
+    "overrides",
+    [
+        {"mode": "bogus"},
+        {"command_port": 0},
+        {"data_port": 70000},
+        {"command_port": 1510, "data_port": 1510},
+        {"publish_rate": 0},
+    ],
+)
+def test_validate_rejects_bad_server_config(overrides):
+    cfg = NatNetInterfaceConfig(**overrides)
+    with pytest.raises(ValueError):
+        cfg.validate()
+
+
+def test_validate_rejects_duplicate_streaming_ids():
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("A", "/World/a", 1),
+            BodyBinding("B", "/World/b", 1),
+        ]
+    )
+    with pytest.raises(ValueError):
+        cfg.validate()
+
+
+def test_validate_rejects_blank_rigid_body_name():
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("", "/World/a", 1)])
+    with pytest.raises(ValueError):
+        cfg.validate()
+
+
+def test_validate_allows_empty_target():
+    # An empty target is valid: a freshly added body to be pointed in the UI/Property panel.
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)])
+    assert cfg.validate() is cfg
+
+
+def test_validate_accepts_good_config():
+    cfg = NatNetInterfaceConfig(
+        bodies=[BodyBinding("Drone", "/World/base_link", 1)]
+    )
+    assert cfg.validate() is cfg
diff --git a/tests/requirements.txt b/tests/requirements.txt
index a4b43e6bb..c752ba8c4 100644
--- a/tests/requirements.txt
+++ b/tests/requirements.txt
@@ -6,3 +6,4 @@ tabulate
 psutil
 pandas
 numpy
+usd-core
diff --git a/tests/sim/README.md b/tests/sim/README.md
index 880930d31..ceae2ebbd 100644
--- a/tests/sim/README.md
+++ b/tests/sim/README.md
@@ -13,4 +13,4 @@ Suggested layout:
 
 | Directory | Purpose |
 |-----------|---------|
-| `optitrack_natnet_emulator/` | NatNet emulator unit tests (proxy; mark: `unit`) |
+| `optitrack_natnet_emulator/` | NatNet emulator unit tests (proxy; mark: `unit`). Includes pure protocol/serializer/config-model checks plus `pxr`-guarded USD interface-authoring tests (skip without `usd-core`). |
diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
new file mode 100644
index 000000000..9c8271bd9
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
@@ -0,0 +1,15 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator USD authoring unit tests.
+
+The underlying tests `pytest.importorskip("pxr")`, so they skip unless USD Python
+(`usd-core` in tests/requirements.txt, or Isaac's bundled USD) is available.
+"""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_interface_authoring.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_config.py b/tests/sim/optitrack_natnet_emulator/test_interface_config.py
new file mode 100644
index 000000000..5e2b139b6
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_interface_config.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator interface config-model unit tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_interface_config.py",
+)

From 95eef1255e9d8bc06f3e7e6f91d5ce4ae6fd41a7 Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Wed, 10 Jun 2026 12:39:27 -0400
Subject: [PATCH 15/27] Scans and detects NatNetInterface object parameters

---
 .../docs/natnet_interface_prim_design.md      | 483 ++++++++++++++++++
 .../natnet/emulator/isaac/__init__.py         |   3 +
 .../natnet/emulator/isaac/manager.py          | 147 ++++++
 .../natnet/emulator/isaac/ui_extension.py     |  12 +
 .../test/test_discovery.py                    |  29 ++
 .../test_discovery.py                         |  11 +
 6 files changed, 685 insertions(+)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_discovery.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
new file mode 100644
index 000000000..a3770f3f8
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
@@ -0,0 +1,483 @@
+# Design: NatNet Interface Prim (stage-driven emulator)
+
+> Status: **proposed** (design only — no implementation yet).
+> Supersedes the hardcoded `defaults.py` `TrackedBodyBinding` approach for the
+> Isaac wrapper. See [`../README.md`](../README.md) for the shipped server.
+> Delivery is staged across four commits — see
+> [Implementation roadmap](#implementation-roadmap-staged-commits).
+
+## Goal
+
+Drive the NatNet emulator entirely from the **USD stage** instead of launch-script
+constants. A config prim in the scene declares *that* a NatNet interface should
+exist, *what* rigid bodies it streams, and *how* the server is configured. The
+extension scans the stage for these prims and manages a server per interface.
+
+Because the configuration lives as authored USD, **the catalog persists between
+runs for free** — it serializes with the `.usd`/`.usda` file. This is the answer
+to "how do we maintain the catalog between runs."
+
+## Why a prim
+
+- **Persistence:** authored attributes save with the scene; reopening the stage
+  restores the full catalog and server config.
+- **Discoverability / authoring:** appears in the Stage tree and Property panel;
+  editable by hand or by tooling without touching Python.
+- **Live control:** toggling an attribute can start/stop the server at runtime.
+- **Multi-interface:** N config prims are supported naturally (multi-robot, or a
+  unicast + multicast pair).
+
+## Schema (USD applied API schema)
+
+Use **applied API schemas** so the attributes are typed and surface cleanly in the
+Property panel. Implement them as **codeless schemas**: author `schema.usda` with
+`skipCodeGeneration = true`, run `usdGenSchema` **once** to emit
+`generatedSchema.usda` + `plugInfo.json` (no C++/Python classes are generated —
+only the registry files), and load that plugin from the extension. USD then reads
+the typed fallbacks at runtime. See [Risks & open questions](#risks--open-questions)
+— schema registration inside Kit is the main unknown and has a fallback.
+
+Two schemas:
+
+### `NatNetInterfaceAPI` (single-apply)
+
+Applied to a holder prim (e.g. a `Scope` or `Xform`, conventionally
+`/World/NatNetInterface`). Carries server-level config:
+
+| Attribute | Type | Maps to `NatNetUnicastServer` |
+|-----------|------|-------------------------------|
+| `natnet:serverEnabled` | `bool` | start / stop (see Lifecycle) |
+| `natnet:serverIp` | `string` | `local_interface` |
+| `natnet:mode` | `token` (`unicast`\|`multicast`) | `transmission_type` |
+| `natnet:multicastAddr` | `string` | `multicast_address` |
+| `natnet:commandPort` | `int` | `command_port` |
+| `natnet:dataPort` | `int` | `data_port` |
+| `natnet:publishRate` | `float` | `publish_rate` |
+| `natnet:natnetVersion` | `string` (`"4.4.0.0"`) | `natnet_version` |
+
+### `NatNetBodyBindingAPI` (multiple-apply)
+
+The **whole catalog lives on the interface prim** — a dictionary of body entries —
+so the extension reads one prim to get the full mapping and never traverses the
+stage to *find* bindings. A **multiple-apply** API schema expresses that dictionary
+natively: one instance per body, keyed by an opaque instance token (`<key>`).
+
+The Motive rigid body name is an **explicit attribute** (`rigidBodyName`), not the
+instance token, so it can be typed/edited as a normal text field in the UI without
+renaming a schema instance. Each instance carries the per-body metadata plus a
+**single-target relationship** to the tracked prim. Relationship targets are
+**path-aware** — USD rewrites them automatically when the tracked prim is renamed or
+reparented — which a literal path-string dict (e.g. `customData` `VtDictionary`)
+would not survive.
+
+| Member (per instance `<key>`) | Type | UI label | Maps to |
+|-------------------------------|------|----------|---------|
+| `natnet:body:<key>:rigidBodyName` | `string` | "Motive Rigid Body Name" | `sRigidBodyDescription.szName` |
+| `natnet:body:<key>:streamingId` | `int` | "Streaming ID" | `sRigidBodyDescription.ID` |
+| `natnet:body:<key>:parentId` | `int` | "Parent ID" | `sRigidBodyDescription.parentID` (default `-1`) |
+| `natnet:body:<key>:target` | `rel` → prim | "Tracked Prim" | pose source |
+
+> Alternative considered: a single relationship `natnet:bodies` with N targets plus
+> parallel `int[]` arrays (`streamingIds`, `parentIds`) indexed alongside the
+> targets. Functionally equivalent and slightly more compact, but the multi-apply
+> instances are typed, individually editable in the Property panel, and self-naming
+> (instance name = body name). Rejected the per-tracked-prim single-apply approach
+> from the prior draft because it forced a stage traversal to discover bindings.
+
+### Example (`.usda` sketch)
+
+```usda
+def Scope "NatNetInterface" (
+    prepend apiSchemas = ["NatNetInterfaceAPI",
+                          "NatNetBodyBindingAPI:body0"]
+)
+{
+    bool   natnet:serverEnabled = true
+    string natnet:serverIp      = "172.31.0.200"
+    token  natnet:mode          = "unicast"
+    int    natnet:commandPort   = 1510
+    int    natnet:dataPort      = 1511
+    float  natnet:publishRate   = 100
+    string natnet:natnetVersion = "4.4.0.0"
+
+    # one body entry (key "body0"); add more by applying NatNetBodyBindingAPI:<key>
+    string natnet:body:body0:rigidBodyName = "Drone"
+    int    natnet:body:body0:streamingId   = 1
+    int    natnet:body:body0:parentId      = -1
+    rel    natnet:body:body0:target        = </World/base_link>
+}
+```
+
+## Catalog assembly
+
+For each enabled interface, read its `NatNetBodyBindingAPI` instances directly off
+the interface prim (no stage traversal), and build one `sRigidBodyDescription` per
+instance (`szName` = `rigidBodyName`, `ID` = streamingId, `parentID` = parentId)
+into an `sDataDescriptions`, then `server.set_model_def_payload(catalog.pack())`.
+This is the data-driven generalization of the current `make_default_drone_catalog()`
+in [`server/natnet_model_types.py`](../optitrack/natnet/emulator/server/natnet_model_types.py).
+
+Resolve each instance's `target` relationship to a `Usd.Prim` handle **once** at
+this point and **cache** the `(prim, bodyName, streamingId)` tuples on the manager
+entry. The per-frame loop then reuses those handles — it never re-reads the schema
+or searches the stage. Rebuild the cache only on enable or when the interface's
+binding members change.
+
+Each interface owns its own dictionary, so multi-interface scoping is inherent —
+no cross-interface grouping key is needed.
+
+## Python API (authoring + scripting)
+
+The prim is the persisted source of truth, but authoring/reading it should never
+require hand-writing `CreateAttribute` / `ApplyAPI` / relationship calls. A thin
+facade in the package wraps the schema so scripts can create, edit, and read an
+interface — and so the same dataclasses are reused by the runtime manager.
+
+### Dataclasses (in-memory form)
+
+```python
+@dataclass
+class BodyBinding:
+    rigid_body_name: str        # Motive rigid body name (szName)
+    target_prim: str            # USD path of the tracked prim
+    streaming_id: int = 1
+    parent_id: int = -1
+
+@dataclass
+class NatNetInterfaceConfig:
+    server_enabled: bool = True
+    server_ip: str = "172.31.0.200"
+    mode: str = "unicast"        # "unicast" | "multicast"
+    multicast_addr: str = "239.255.42.99"
+    command_port: int = 1510
+    data_port: int = 1511
+    publish_rate: float = 100.0
+    natnet_version: str = "4.4.0.0"
+    bodies: list[BodyBinding] = field(default_factory=list)
+
+    @classmethod
+    def from_dict(cls, data: dict) -> "NatNetInterfaceConfig": ...
+    def to_dict(self) -> dict: ...
+```
+
+`from_dict` accepts the dictionary the user passes in a script. `bodies` may be a
+list of dicts **or** a `{prim_path: {...}}` mapping (the "dictionary of prims →
+rigid body names and stuff"), normalized into `BodyBinding` objects.
+
+### Authoring / reading helpers
+
+```python
+def author_interface(stage, prim_path, config) -> Usd.Prim:
+    """Create/overwrite the interface prim: apply NatNetInterfaceAPI, set server
+    attrs, and apply one NatNetBodyBindingAPI:<name> instance per body with its
+    target relationship. `config` may be a NatNetInterfaceConfig or a plain dict."""
+
+def read_interface(prim) -> NatNetInterfaceConfig:
+    """Reverse of author_interface — reconstruct the dataclass from authored USD."""
+
+def find_interfaces(stage) -> list[Usd.Prim]:
+    """All prims carrying NatNetInterfaceAPI (used by the manager and tooling)."""
+```
+
+### Script usage (standalone launch)
+
+Author from a dict and bring the server up in one place — no GUI required:
+
+```python
+from optitrack.natnet.emulator.isaac import author_interface, NatNetServerManager
+
+CONFIG = {
+    "server_enabled": True,
+    "server_ip": "172.31.0.200",
+    "mode": "unicast",
+    "publish_rate": 100,
+    "bodies": {
+        "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1},
+    },
+}
+
+author_interface(stage, "/World/NatNetInterface", CONFIG)   # persists with the stage
+
+manager = NatNetServerManager(stage)   # discovers interfaces, honors serverEnabled
+manager.start()                        # spins up servers + per-frame pose sampling
+# manager.shutdown() on teardown
+```
+
+Because `author_interface` writes the same attributes the GUI/manager read, a
+script-authored interface is indistinguishable from a hand-authored one and saves
+into the `.usd`. A convenience `author_and_start(...)` can wrap the two calls for
+the common single-interface script case.
+
+> The dataclasses (`BodyBinding`, `NatNetInterfaceConfig`) are the single in-memory
+> representation shared by: `from_dict` (script input) → `author_interface` (USD
+> write) → `read_interface` (USD read) → catalog assembly + pose-sampling cache.
+> This is also the natural successor to `defaults.py` — its constants become a
+> `NatNetInterfaceConfig` default.
+
+## Lifecycle (manager)
+
+A manager owns a map `{Sdf.Path -> NatNetUnicastServer}` keyed by interface prim
+path, and reacts to a `Usd.Notice.ObjectsChanged` listener plus stage-open events.
+
+- `natnet:serverEnabled` → **true**: construct a **fresh** `NatNetUnicastServer` from
+  the prim's attributes, build + set the MODELDEF payload, `start()`, store in the map.
+- `natnet:serverEnabled` → **false**: `server.shutdown()` (closes both sockets, joins
+  all child threads), remove from the map.
+- Server-config attr changes while enabled (IP, ports, mode): treat as shutdown +
+  fresh start so the new socket binding takes effect.
+- Binding-member changes while enabled (add/remove a body, edit a `target`,
+  streamingId, or parentId): rebuild the cached `(prim, bodyName, streamingId)`
+  list and refresh the MODELDEF via `set_model_def_payload()` — **no server restart
+  needed**, since the transport/sockets are unchanged.
+
+**Fresh instance per enable (chosen):** the manager always builds a new server
+object on enable and discards the old one. The existing server is not designed to
+be re-`start()`ed in place — `shutdown()` closes sockets without recreating them and
+`start()` appends to `self.threads` without resetting — so reusing an instance across
+a disable→enable cycle would re-run dead threads with `shutdown_event` still set.
+A fresh instance avoids all of that with no server-side changes. Note this is
+**thread** teardown within the Kit process — not an OS process kill: `shutdown()`
+ends the server's daemon threads and closes its sockets:
+
+```text
+shutdown(): running=False; shutdown_event.set(); close command_socket; close data_socket; join threads
+```
+
+## Pose sampling
+
+A per-frame Kit update callback iterates the **cached** `(prim, bodyName,
+streamingId)` tuples (resolved once during catalog assembly), reads each prim's
+world transform (`UsdGeom.Xformable.ComputeLocalToWorldTransform` via a per-frame
+`UsdGeomXformCache`, or the physics view pose), converts it into an
+`sFrameOfMocapData` rigid body (params `& 0x01` = tracking valid), and calls
+`server.enqueue_mocap_data(frame)`. No schema reads or stage searches happen in the
+hot path — only direct pose pulls off the cached handles, exactly as intended.
+
+**Frame convention caveat:** Isaac is typically Z-up / meters; Motive streams Y-up
+by convention. The sampler must emit the same convention `natnet_ros2` expects on
+the wire — to be pinned down during implementation (not a design blocker).
+
+## Packaging notes
+
+- **Codeless applied schemas** keep this dependency-light: `schema.usda` defines
+  `NatNetInterfaceAPI` / `NatNetBodyBindingAPI` with `skipCodeGeneration = true`;
+  `usdGenSchema` emits `generatedSchema.usda` + `plugInfo.json` (registry only, no
+  compiled classes); the extension loads the plugin on startup. Registration
+  reliability inside Kit is the main risk — see below.
+- **Standalone + Kit parity:** put "scan stage + manage servers + sample poses" in
+  a plain module callable from both the Kit `omni.ext.IExt` (GUI/live toggle) and
+  the standalone launch scripts (`launch_scripts/example_*_pegasus_launch_script.py`),
+  which have no extension lifecycle.
+- **Persistence layer:** author interface/binding attributes on the stage's
+  root/edit layer (not the session layer) so they save with the scene.
+
+## Implementation roadmap (staged commits)
+
+Each stage is independently reviewable and ends with a concrete manual check.
+Stages build on each other **in order** — each depends on the previous:
+
+1. **Commit 1 — Schema + config prim spawn + authoring UI** (no deps)
+2. **Commit 2 — Detection + parameter read** (needs the schema + facade from 1)
+3. **Commit 3 — Catalog parse + server start** (needs detection/read from 2)
+4. **Commit 4 — Enable/disable lifecycle** (needs the start path from 3)
+
+Unit tests are added **within each commit** (co-located in the extension `test/`
+dir with a proxy under `tests/sim/`), so every stage lands already covered — see
+[Testing & CI integration](#testing--ci-integration) for the mechanism.
+
+> **Design for testability:** keep a pure-Python **config model** (dataclasses,
+> `from_dict`/`to_dict`, the `natnet:body:<key>:…` attribute-name builder,
+> validation) separate from a thin **USD binding** layer (apply/read against a
+> `Usd.Stage`). Most logic is then hermetically testable with no `pxr`/Isaac, and
+> the USD shell stays small and `pxr`-guarded.
+
+### Commit 1 — Schema + config prim spawn + authoring UI
+
+Stand up the data model and let a user create/edit an interface by hand. No
+scanning, no server.
+
+- **Schema:** `schema.usda` + `plugInfo.json` defining the codeless
+  `NatNetInterfaceAPI` (single-apply) and `NatNetBodyBindingAPI` (multiple-apply);
+  registered/loaded via `config/extension.toml`.
+- **Authoring facade:** `BodyBinding` / `NatNetInterfaceConfig` dataclasses,
+  `author_interface()`, and add/remove-body helpers (no `read_*`/manager yet).
+- **UI (Kit extension):** a menu entry (e.g. *Create ▸ NatNet Interface*) that
+  spawns `/World/NatNetInterface`; a panel to edit the server attributes and to
+  **add a tracked body from the current stage selection**, typing *Motive Rigid
+  Body Name*, *Streaming ID*, *Parent ID* (the `target` relationship is set from
+  the selected prim).
+- 🧪 **Tests** (`test/test_config_model.py`, hermetic): `from_dict`/`to_dict`
+  round-trips; `bodies` normalization from a list **and** a `{prim_path: {...}}`
+  mapping; the `natnet:body:<key>:…` attribute-name builder; validation
+  (mode ∈ {unicast, multicast}, distinct command/data ports). Plus
+  `test/test_authoring_usd.py` (`pytest.importorskip("pxr")`):
+  `author_interface()` → `read_interface()` round-trip on
+  `Usd.Stage.CreateInMemory()`, asserting attrs, the multi-apply instances, and the
+  `target` relationship.
+- ✅ **Manual verify:** create the prim from the menu, select a prim, fill the
+  fields, **save and reopen** the stage — the attributes and the `target`
+  relationship persist and match what was entered.
+
+### Commit 2 — Detection + parameter read
+
+Make the extension *aware* of interfaces without acting on them.
+
+- `NatNetServerManager` skeleton: `find_interfaces()`, `read_interface() ->
+  NatNetInterfaceConfig`, a `Usd.Notice.ObjectsChanged` listener, and stage-open
+  hookup. Logs each detected interface and its parsed config (server params + body
+  list). No server, no pose sampling.
+- 🧪 **Tests** (`test/test_discovery.py`): pure `reconcile(old, new) -> actions`
+  diff logic (hermetic) deciding start/stop/rebuild from config deltas;
+  `find_interfaces()` on an in-memory stage with/without the API applied
+  (`importorskip("pxr")`).
+- ✅ **Manual verify:** open a stage containing an interface → logs show the prim
+  path and fully parsed config; adding/removing a body or editing a field updates
+  the logged readout live.
+
+### Commit 3 — Catalog parse + server start
+
+Turn a detected config into a running server.
+
+- Build `sDataDescriptions` from the config, resolve each `target` to a `Usd.Prim`
+  and cache `(prim, rigidBodyName, streamingId)`, construct a `NatNetUnicastServer`
+  from the server attrs, `set_model_def_payload()`, `start()`, and run the per-frame
+  pose-sampling callback → `enqueue_mocap_data()`.
+- Initial `serverEnabled` is honored at load (start only if true); **live toggling
+  is deferred to Commit 4.**
+- 🧪 **Tests** (hermetic): `test/test_catalog_from_config.py` — a
+  `NatNetInterfaceConfig` with N bodies packs into an `sDataDescriptions` with the
+  right `szName`/`ID`/`parentID` per body (extends the existing catalog tests);
+  `test/test_pose_to_frame.py` — pure `pose → sFrameOfMocapData` conversion incl.
+  the **Z-up → Y-up** frame convention (feed a known transform, assert position +
+  quaternion). Plus a loopback case extending `test_unicast_protocol.py`: a server
+  built from a config serves that MODELDEF and streams enqueued frames.
+- ✅ **Manual verify:** with `serverEnabled = true` at load, the robot
+  `natnet_ros2` connects and `/{ROBOT_NAME}/perception/optitrack/<name>` publishes
+  at ~`publishRate`; moving the tracked prim moves the published pose. (A Python
+  loopback client can stand in for `natnet_ros2` if the SDK isn't built.)
+
+### Commit 4 — Enable/disable lifecycle
+
+React to `serverEnabled` flipping at runtime.
+
+- On `serverEnabled` **false** → `server.shutdown()` (closes sockets, joins all
+  child threads), drop from the map. On **true** → build a **fresh**
+  `NatNetUnicastServer` and `start()` again. (Server-config and binding live-edits
+  per the [Lifecycle](#lifecycle-manager) section land here or as an immediate
+  follow-up.)
+- 🧪 **Tests** (`test/test_lifecycle.py`, hermetic): the manager state machine
+  (`serverEnabled` true→false→true yields stop then fresh-start actions); and a
+  restart-cleanliness loopback test — `start()` then `shutdown()` joins all threads
+  and closes both sockets, and a **fresh** instance rebinds the same ports and
+  serves again (proves no thread/socket leak across cycles).
+- ✅ **Manual verify:** toggle `serverEnabled` in the UI repeatedly — the server
+  threads die and respawn cleanly, the pose topic stops and resumes, and there is
+  no thread/socket leak across cycles (guaranteed by the fresh-instance approach).
+
+## Testing & CI integration
+
+Follows AirStack's **co-location + proxy** unit-test pattern (see the
+[`add-unit-tests`](../../../../../.agents/skills/add-unit-tests/SKILL.md) skill).
+The extension already uses it today — test source in
+[`test/`](../test/), thin proxies under
+[`tests/sim/optitrack_natnet_emulator/`](../../../../../tests/sim/optitrack_natnet_emulator/).
+
+**Mechanism (per new test file):**
+
+1. Write the test co-located in
+   `simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_<name>.py`,
+   decorated `@pytest.mark.unit`.
+2. Add a one-line proxy
+   `tests/sim/optitrack_natnet_emulator/test_<name>.py` that calls
+   `reexport_unit_tests(globals(), repo_path(".../optitrack.natnet.emulator/test"), "test_<name>.py")`.
+3. It is then discovered automatically by `pytest tests/ -m unit`,
+   `airstack test -m unit`, and CI `system-tests.yml` (PR-open runs `pytest tests/`
+   with no `-m` filter) — **no CI YAML change needed.**
+
+**Two specifics for this feature:**
+
+- **`usd-core` dependency.** The `pxr`-guarded tests (`author_interface` /
+  `read_interface` / `find_interfaces` against an in-memory stage) only execute if
+  `pxr` is importable; otherwise `pytest.importorskip("pxr")` skips them. To make CI
+  actually exercise them, add **`usd-core`** to
+  [`tests/requirements.txt`](../../../../../tests/requirements.txt) (installed into
+  the venv by `system-tests.yml`). Pure config-model / catalog / pose / lifecycle
+  tests need no new deps (`numpy` is already present).
+- **Not a colcon package.** This extension is a sim-side Python extension, **not** an
+  ament/colcon package, so it does **not** go in
+  [`tests/colcon_unit_test_packages.yaml`](../../../../../tests/colcon_unit_test_packages.yaml).
+  Its CI coverage is the pytest proxy route only (the `build_packages` colcon route
+  does not apply).
+
+**Per-commit test artifacts:**
+
+| Commit | Co-located test file(s) | `pxr`? | Proxy to add |
+|--------|-------------------------|--------|--------------|
+| 1 | `test_config_model.py`, `test_authoring_usd.py` | model: no · authoring: yes | one proxy each |
+| 2 | `test_discovery.py` | `find_interfaces`: yes · reconcile: no | one proxy |
+| 3 | `test_catalog_from_config.py`, `test_pose_to_frame.py` (+ extend `test_unicast_protocol.py`) | no | one proxy each |
+| 4 | `test_lifecycle.py` | no | one proxy |
+
+Update [`tests/sim/README.md`](../../../../../tests/sim/README.md) as proxies are
+added. Run locally with `airstack test -m unit -v` or
+`pytest simulation/isaac-sim/extensions/optitrack.natnet.emulator/test -m unit -v`.
+
+## Risks & open questions
+
+Ordered roughly by impact. The first two change *how* Commit 1 / Commit 3 are
+sequenced; the rest are "note and handle during implementation."
+
+1. **Schema registration inside Kit (highest unknown).** Codeless schemas still
+   require `usdGenSchema`-emitted `generatedSchema.usda` + `plugInfo.json` to be
+   discovered by USD's plugin system (search paths, `HasAPI`/`ApplyAPI`, typed
+   fallbacks) within the Kit runtime. **Mitigation:** make a **schema-registration
+   spike the first task of Commit 1**, before any UI. **Fallback:** if codeless
+   registration is painful in this Kit/USD version, drop to plain namespaced custom
+   attributes (`prim.CreateAttribute("natnet:…")`) — the Python facade hides which
+   backing is used, so runtime/UI code is unaffected.
+
+2. **Commit 1 front-loads risk with no streaming payoff.** Schema + facade + custom
+   multi-apply UI is heavy and moves no poses. **Option:** prove
+   schema + config model + facade + headless manager/server via the **script path +
+   unit tests first**, then build the Kit GUI on that tested foundation. Keeps the
+   "manually verify the UI" deliverable but rests it on green tests. *(Sequencing
+   choice — not yet decided.)*
+
+3. **Multi-apply + relationship UI is the heavy part.** The relationship picker and
+   "add body from selection" are the real cost in Commit 1. Consider shipping v1
+   with a **single body** (or the relationship + parallel-arrays variant) to close
+   the end-to-end loop first, then generalize the catalog UI.
+
+4. **Frame convention is a correctness landmine (not a footnote).** Isaac Z-up /
+   meters vs Motive Y-up will silently produce wrong poses, and `natnet_ros2`
+   *already* converts — so the risk is **double-converting**. **Make pinning the
+   exact wire convention a first-class task in Commit 3**, validated against the
+   existing verified `libNatNet` contract and `tests/integration/natnet/`.
+
+5. **Pose-sampling timing.** The world-transform read must happen **after PhysX
+   write-back** (the correct post-physics / world callback), or streamed poses lag
+   or read stale. Confirm which callback the per-frame loop hooks into.
+
+6. **Rapid enable toggling can race the socket rebind.** Fresh-instance +
+   `shutdown()` join (1 s) then re-`start()` on the same port can collide if the old
+   socket has not fully closed. `SO_REUSEADDR` is set and the restart-cleanliness
+   test (Commit 4) guards this; consider also **debouncing** rapid toggles.
+
+7. **Standalone vs Kit parity for the live toggle.** `serverEnabled` live-toggling
+   relies on the `ObjectsChanged` loop, which only runs under Kit. Standalone
+   scripts use the **initial** state unless they pump USD notices — document this.
+
+8. **`usd-core` ≠ Isaac's bundled USD.** The CI `pxr` from `usd-core` may differ in
+   version from Isaac's USD. Fine for hermetic schema/Sdf unit tests; just be aware
+   behavior can differ slightly from the runtime.
+
+9. **Release gating.** Docker image content changes here (extension install), so bump
+   `.env` `VERSION` + CHANGELOG before merge (see `bump-version-and-release`).
+
+## Relationship to existing code
+
+- Replaces `defaults.py` `TrackedBodyBinding` (each `NatNetBodyBindingAPI` instance
+  is the authored form of that dataclass; `prim_path` becomes the instance's
+  `target` relationship).
+- Server (`NatNetUnicastServer`) is unchanged — it stays transport-only and
+  consumes `set_model_def_payload()` + `enqueue_mocap_data()` exactly as today.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
index 62b9882e9..41ae8d763 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -13,6 +13,7 @@
     body_attr_name,
     make_instance_key,
 )
+from .manager import NatNetServerManager, format_interface
 from .usd_bindings import (
     author_interface,
     find_interfaces,
@@ -23,9 +24,11 @@
 __all__ = [
     "BodyBinding",
     "NatNetInterfaceConfig",
+    "NatNetServerManager",
     "author_interface",
     "body_attr_name",
     "find_interfaces",
+    "format_interface",
     "is_interface",
     "make_instance_key",
     "read_interface",
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
new file mode 100644
index 000000000..a5268dcdd
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -0,0 +1,147 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commit 2 — detection + parameter read.
+
+``NatNetServerManager`` makes the extension *aware* of interface prims without
+acting on them yet: it scans the stage, reads each interface's config, and prints
+the parsed parameters. It re-scans when the stage is opened and when a NatNet prim
+changes (``Usd.Notice.ObjectsChanged``). No server, no pose sampling — those come
+in Commits 3 and 4.
+
+``format_interface`` is a pure function (no USD) so it's trivially testable. All
+``pxr`` / ``omni`` imports are lazy so importing this module stays hermetic.
+"""
+
+from __future__ import annotations
+
+from .config import NatNetInterfaceConfig
+from .usd_bindings import find_interfaces, read_interface
+
+
+def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str:
+    """Render a human-readable multi-line summary of one interface config."""
+    lines = [f"[natnet] Interface @ {prim_path}"]
+    lines.append(f"  serverEnabled : {cfg.server_enabled}")
+    lines.append(f"  serverIp      : {cfg.server_ip}")
+    lines.append(f"  mode          : {cfg.mode}")
+    if cfg.mode == "multicast":
+        lines.append(f"  multicastAddr : {cfg.multicast_addr}")
+    lines.append(f"  commandPort   : {cfg.command_port}")
+    lines.append(f"  dataPort      : {cfg.data_port}")
+    lines.append(f"  publishRate   : {cfg.publish_rate}")
+    lines.append(f"  natnetVersion : {cfg.natnet_version}")
+    if cfg.bodies:
+        lines.append(f"  bodies ({len(cfg.bodies)}):")
+        for b in cfg.bodies:
+            target = b.target_prim or "<no target>"
+            lines.append(
+                f"    - {b.rigid_body_name} (id={b.streaming_id}, parent={b.parent_id}) -> {target}"
+            )
+    else:
+        lines.append("  bodies        : (none)")
+    return "\n".join(lines)
+
+
+class NatNetServerManager:
+    """Detects interface prims and prints their parameters on stage/USD changes."""
+
+    def __init__(self):
+        self._stage_event_sub = None
+        self._usd_listener = None
+        self._scan_tick_sub = None
+        self._scan_pending = False
+
+    # --- lifecycle -------------------------------------------------------------
+
+    def on_startup(self):
+        import omni.usd
+
+        usd_context = omni.usd.get_context()
+        self._stage_event_sub = usd_context.get_stage_event_stream().create_subscription_to_pop(
+            self._on_stage_event, name="natnet_manager_stage_events"
+        )
+        self._register_usd_listener()
+        print("[natnet] NatNetServerManager initialized")
+        self.scan_and_print()
+
+    def on_shutdown(self):
+        self._stage_event_sub = None
+        self._scan_tick_sub = None
+        self._scan_pending = False
+        self._revoke_usd_listener()
+
+    # --- scanning --------------------------------------------------------------
+
+    def scan_and_print(self, *_):
+        """Find every interface prim and print its parsed config."""
+        stage = self._get_stage()
+        if stage is None:
+            return
+        interfaces = find_interfaces(stage)
+        if not interfaces:
+            print("[natnet] Scan: no NatNetInterface prims on stage.")
+            return
+        print(f"[natnet] Scan: {len(interfaces)} interface(s) detected.")
+        for prim in interfaces:
+            cfg = read_interface(prim)
+            print(format_interface(prim.GetPath().pathString, cfg))
+
+    # --- stage / USD notifications --------------------------------------------
+
+    def _get_stage(self):
+        import omni.usd
+
+        return omni.usd.get_context().get_stage()
+
+    def _on_stage_event(self, event):
+        import omni.usd
+
+        if event.type == int(omni.usd.StageEventType.OPENED):
+            self._register_usd_listener()
+            self.scan_and_print()
+
+    def _register_usd_listener(self):
+        from pxr import Tf, Usd
+
+        stage = self._get_stage()
+        if stage is None:
+            return
+        self._revoke_usd_listener()
+        self._usd_listener = Tf.Notice.Register(
+            Usd.Notice.ObjectsChanged, self._on_objects_changed, stage
+        )
+
+    def _revoke_usd_listener(self):
+        if self._usd_listener is not None:
+            self._usd_listener.Revoke()
+            self._usd_listener = None
+
+    def _on_objects_changed(self, notice, sender):
+        # Only re-scan when something NatNet-related changed, so we don't spam the
+        # console on every transform update while the sim is playing.
+        try:
+            paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths())
+        except Exception:  # pragma: no cover - defensive
+            paths = []
+        if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths):
+            # A single author_interface() (Create/Save) emits many notices — one per
+            # attribute/relationship op. Debounce them into one scan on the next
+            # update tick so we print the final state once, not once per op.
+            self._request_scan()
+
+    def _request_scan(self):
+        if self._scan_pending:
+            return
+        self._scan_pending = True
+        import omni.kit.app
+
+        self._scan_tick_sub = (
+            omni.kit.app.get_app()
+            .get_update_event_stream()
+            .create_subscription_to_pop(self._on_scan_tick, name="natnet_manager_scan_tick")
+        )
+
+    def _on_scan_tick(self, _event):
+        self._scan_pending = False
+        self._scan_tick_sub = None
+        self.scan_and_print()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index df0496736..ce89f44cc 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -26,6 +26,7 @@
 import omni.ext
 
 from .config import VALID_MODES, BodyBinding, NatNetInterfaceConfig
+from .manager import NatNetServerManager
 from .usd_bindings import author_interface, find_interfaces, read_interface
 
 _DEFAULT_PRIM_PATH = "/World/NatNetInterface"
@@ -39,10 +40,15 @@ def on_startup(self, ext_id):  # noqa: D401 - Kit lifecycle hook
         self._window = None
         self._bodies_frame = None
         self._cfg = NatNetInterfaceConfig()
+        self._manager = NatNetServerManager()
+        self._manager.on_startup()
         self._add_menu()
 
     def on_shutdown(self):
         self._remove_menu()
+        if self._manager is not None:
+            self._manager.on_shutdown()
+            self._manager = None
         if self._window is not None:
             self._window.destroy()
             self._window = None
@@ -98,6 +104,7 @@ def _build_window(self):
                     ui.Button("Create Server", clicked_fn=self._create_server)
                     ui.Button("Save", clicked_fn=self._save)
                     ui.Button("Load from Stage", clicked_fn=self._load_from_stage)
+                    ui.Button("Print config", clicked_fn=self._print_config)
 
                 ui.Label(
                     "\u26a0  Remember to save after each edit",
@@ -324,6 +331,11 @@ def _load_from_stage(self):
             carb.log_info(f"[natnet] Loaded interface from {prim.GetPath().pathString}.")
         self._refresh()
 
+    def _print_config(self):
+        # Print whatever is authored on the stage (the source of truth).
+        if self._manager is not None:
+            self._manager.scan_and_print()
+
     def _create_server(self):
         import carb
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
new file mode 100644
index 000000000..642615e9f
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
@@ -0,0 +1,29 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Light unit tests for Commit 2 detection/printing (the pure formatter)."""
+
+from __future__ import annotations
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig
+from optitrack.natnet.emulator.isaac.manager import format_interface
+
+pytestmark = pytest.mark.unit
+
+
+def test_format_interface_includes_server_params_and_bodies():
+    cfg = NatNetInterfaceConfig(
+        server_ip="10.0.0.5",
+        bodies=[BodyBinding("Drone", "/World/base_link", 1)],
+    )
+    text = format_interface("/World/NatNetInterface", cfg)
+    assert "/World/NatNetInterface" in text
+    assert "10.0.0.5" in text
+    assert "Drone" in text
+    assert "/World/base_link" in text
+
+
+def test_format_interface_reports_no_bodies():
+    text = format_interface("/World/NatNetInterface", NatNetInterfaceConfig())
+    assert "(none)" in text
diff --git a/tests/sim/optitrack_natnet_emulator/test_discovery.py b/tests/sim/optitrack_natnet_emulator/test_discovery.py
new file mode 100644
index 000000000..b1d43de2b
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_discovery.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator Commit 2 detection unit tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_discovery.py",
+)

From a71655e42ecb499fe2676edb4deddbb2569a68cc Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Wed, 10 Jun 2026 13:02:46 -0400
Subject: [PATCH 16/27] Implemented server starting and stopping

---
 .../natnet/emulator/isaac/__init__.py         |   8 +-
 .../natnet/emulator/isaac/catalog.py          |  54 ++++++
 .../natnet/emulator/isaac/manager.py          | 133 +++++++++++++--
 .../natnet/emulator/isaac/ui_extension.py     |  36 +++-
 .../natnet/emulator/isaac/usd_bindings.py     |  17 ++
 .../test/test_catalog.py                      | 115 +++++++++++++
 .../test/test_server_from_config.py           |  91 ++++++++++
 .../test/test_server_lifecycle.py             | 161 ++++++++++++++++++
 .../test/test_target_resolution.py            |  90 ++++++++++
 .../optitrack_natnet_emulator/test_catalog.py |  11 ++
 .../test_server_from_config.py                |  11 ++
 .../test_server_lifecycle.py                  |  11 ++
 .../test_target_resolution.py                 |  11 ++
 13 files changed, 734 insertions(+), 15 deletions(-)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_catalog.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_server_from_config.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_target_resolution.py

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
index 41ae8d763..4bc914951 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -13,12 +13,14 @@
     body_attr_name,
     make_instance_key,
 )
-from .manager import NatNetServerManager, format_interface
+from .catalog import build_catalog, find_duplicate_targets
+from .manager import NatNetServerManager, default_server_factory, format_interface
 from .usd_bindings import (
     author_interface,
     find_interfaces,
     is_interface,
     read_interface,
+    resolve_targets,
 )
 
 __all__ = [
@@ -27,9 +29,13 @@
     "NatNetServerManager",
     "author_interface",
     "body_attr_name",
+    "build_catalog",
+    "default_server_factory",
+    "find_duplicate_targets",
     "find_interfaces",
     "format_interface",
     "is_interface",
     "make_instance_key",
     "read_interface",
+    "resolve_targets",
 ]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
new file mode 100644
index 000000000..f83a0eb51
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
@@ -0,0 +1,54 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commit 3 — catalog parse.
+
+Turn a :class:`NatNetInterfaceConfig` into the server's MODELDEF catalog
+(``sDataDescriptions`` of rigid bodies). Pure Python + ctypes (the ``server``
+package is stdlib-only), so this is hermetically unit-testable — no USD, no Kit.
+"""
+
+from __future__ import annotations
+
+from ..server.natnet_common import ModelLimits
+from ..server.natnet_model_types import DataDescriptors, sDataDescriptions
+from .config import NatNetInterfaceConfig
+
+# szName is null-terminated on the wire; reserve one byte for the terminator.
+_MAX_NAME_BYTES = int(ModelLimits.MAX_NAMELENGTH) - 1
+_MAX_MODELS = int(ModelLimits.MAX_MODELS)
+
+
+def build_catalog(config: NatNetInterfaceConfig) -> sDataDescriptions:
+    """Build an ``sDataDescriptions`` rigid-body catalog from the config bodies.
+
+    No bodies -> an empty catalog (``nDataDescriptions == 0``). Names longer than
+    the NatNet name field are truncated. Raises ``ValueError`` if there are more
+    bodies than the protocol allows.
+    """
+    bodies = config.bodies
+    if len(bodies) > _MAX_MODELS:
+        raise ValueError(
+            f"Too many bodies for one catalog: {len(bodies)} > {_MAX_MODELS} (MAX_MODELS)"
+        )
+
+    descriptions = sDataDescriptions()
+    descriptions.nDataDescriptions = len(bodies)
+    for i, body in enumerate(bodies):
+        desc = descriptions.arrDataDescriptions[i]
+        desc.type = int(DataDescriptors.Descriptor_RigidBody)
+        rb = desc.RigidBodyDescription
+        rb.szName = body.rigid_body_name.encode("utf-8")[:_MAX_NAME_BYTES]
+        rb.ID = int(body.streaming_id)
+        rb.parentID = int(body.parent_id)
+        rb.offsetqw = 1.0  # identity quaternion offset
+        rb.nMarkers = 0
+    return descriptions
+
+
+def find_duplicate_targets(config: NatNetInterfaceConfig) -> list[str]:
+    """Return target prim paths referenced by more than one body (empties ignored)."""
+    counts: dict[str, int] = {}
+    for body in config.bodies:
+        if body.target_prim:
+            counts[body.target_prim] = counts.get(body.target_prim, 0) + 1
+    return [path for path, count in counts.items() if count > 1]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
index a5268dcdd..e473e6c35 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -1,21 +1,52 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commit 2 — detection + parameter read.
-
-``NatNetServerManager`` makes the extension *aware* of interface prims without
-acting on them yet: it scans the stage, reads each interface's config, and prints
-the parsed parameters. It re-scans when the stage is opened and when a NatNet prim
-changes (``Usd.Notice.ObjectsChanged``). No server, no pose sampling — those come
-in Commits 3 and 4.
-
-``format_interface`` is a pure function (no USD) so it's trivially testable. All
-``pxr`` / ``omni`` imports are lazy so importing this module stays hermetic.
+"""Commits 2-4 — detection, parameter read, and server start/stop lifecycle.
+
+``NatNetServerManager`` detects interface prims, prints their parsed config
+(Commit 2), and owns a **single** server instance it can start and stop (Commits
+3-4). On each enable it builds a MODELDEF catalog from the config and constructs a
+fresh server via an injectable factory (so unit tests can mock it and assert the
+server is created/started exactly once, without binding real sockets). Pose
+sampling / frame publishing is intentionally left for a later commit.
+
+``format_interface`` is pure (no USD). The server factory and lifecycle are
+USD-free too (they take a ``NatNetInterfaceConfig``); only the stage-driven
+helpers touch ``pxr`` / ``omni`` (lazily), so importing this module stays hermetic.
 """
 
 from __future__ import annotations
 
+from .catalog import build_catalog, find_duplicate_targets
 from .config import NatNetInterfaceConfig
-from .usd_bindings import find_interfaces, read_interface
+from .usd_bindings import find_interfaces, read_interface, resolve_targets
+
+
+def _parse_version(version_str: str) -> tuple[int, int, int, int]:
+    try:
+        parts = tuple(int(x) for x in str(version_str).split("."))
+    except ValueError:
+        parts = ()
+    return (parts + (0, 0, 0, 0))[:4]
+
+
+def default_server_factory(config: NatNetInterfaceConfig):
+    """Construct (but do not start) a ``NatNetUnicastServer`` from a config."""
+    from ..server import NatNetUnicastServer, TransmissionType
+
+    if config.mode != "unicast":
+        raise NotImplementedError(
+            f"mode {config.mode!r} is not supported yet (unicast only)"
+        )
+    server = NatNetUnicastServer(
+        local_interface=config.server_ip,
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=config.command_port,
+        data_port=config.data_port,
+    )
+    server.publish_rate = config.publish_rate
+    server.natnet_version = _parse_version(config.natnet_version)
+    return server
 
 
 def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str:
@@ -43,13 +74,15 @@ def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str:
 
 
 class NatNetServerManager:
-    """Detects interface prims and prints their parameters on stage/USD changes."""
+    """Detects interface prims, prints config, and owns one server instance."""
 
-    def __init__(self):
+    def __init__(self, server_factory=None):
         self._stage_event_sub = None
         self._usd_listener = None
         self._scan_tick_sub = None
         self._scan_pending = False
+        self._server = None
+        self._server_factory = server_factory or default_server_factory
 
     # --- lifecycle -------------------------------------------------------------
 
@@ -65,6 +98,7 @@ def on_startup(self):
         self.scan_and_print()
 
     def on_shutdown(self):
+        self.stop_server()
         self._stage_event_sub = None
         self._scan_tick_sub = None
         self._scan_pending = False
@@ -86,6 +120,79 @@ def scan_and_print(self, *_):
             cfg = read_interface(prim)
             print(format_interface(prim.GetPath().pathString, cfg))
 
+    # --- server lifecycle (single instance; USD-free, factory-injectable) ------
+
+    @property
+    def is_running(self) -> bool:
+        return self._server is not None
+
+    @property
+    def server(self):
+        return self._server
+
+    def start_server(self, config: NatNetInterfaceConfig) -> bool:
+        """Build the catalog, construct a fresh server, and start it — once.
+
+        Idempotent: if a server is already running this is a no-op returning False.
+        Returns True when a new server was created and started.
+        """
+        if self._server is not None:
+            print("[natnet] start_server ignored: a server is already running.")
+            return False
+        catalog = build_catalog(config)
+        server = self._server_factory(config)
+        server.set_model_def_payload(catalog.pack())
+        server.start()
+        self._server = server
+        print(
+            f"[natnet] Server started on {config.server_ip} "
+            f"(cmd {config.command_port} / data {config.data_port}) "
+            f"with {len(config.bodies)} body(ies)."
+        )
+        return True
+
+    def stop_server(self) -> bool:
+        """Shut down the running server (fresh instance is built on next start).
+
+        Idempotent: returns False if nothing was running.
+        """
+        if self._server is None:
+            return False
+        try:
+            self._server.shutdown()
+        finally:
+            self._server = None
+        print("[natnet] Server stopped.")
+        return True
+
+    def toggle_server(self, config: NatNetInterfaceConfig) -> bool:
+        """Start if stopped, stop if running. Returns the resulting running state."""
+        if self.is_running:
+            self.stop_server()
+        else:
+            self.start_server(config)
+        return self.is_running
+
+    def apply_enabled(self, config: NatNetInterfaceConfig) -> None:
+        """Reconcile running state to ``config.server_enabled`` (start/stop)."""
+        if config.server_enabled and not self.is_running:
+            self.start_server(config)
+        elif not config.server_enabled and self.is_running:
+            self.stop_server()
+
+    def log_target_diagnostics(self, config: NatNetInterfaceConfig) -> None:
+        """Warn about missing target prims and duplicate targets (best-effort)."""
+        stage = self._get_stage()
+        if stage is not None:
+            _existing, missing = resolve_targets(stage, config)
+            for body in missing:
+                print(
+                    f"[natnet] WARNING: body '{body.rigid_body_name}' target prim "
+                    f"missing or empty: {body.target_prim or '<empty>'}"
+                )
+        for path in find_duplicate_targets(config):
+            print(f"[natnet] WARNING: multiple bodies target the same prim: {path}")
+
     # --- stage / USD notifications --------------------------------------------
 
     def _get_stage(self):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index ce89f44cc..afe84e553 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -101,11 +101,23 @@ def _build_window(self):
                 ui.Label("NatNet interface", height=0, style={"font_size": 16})
 
                 with ui.HStack(height=28, spacing=6):
-                    ui.Button("Create Server", clicked_fn=self._create_server)
+                    ui.Button("Create Interface", clicked_fn=self._create_server)
                     ui.Button("Save", clicked_fn=self._save)
                     ui.Button("Load from Stage", clicked_fn=self._load_from_stage)
                     ui.Button("Print config", clicked_fn=self._print_config)
 
+                running = self._manager is not None and self._manager.is_running
+                with ui.HStack(height=28, spacing=6):
+                    ui.Button(
+                        "Stop Server" if running else "Start Server",
+                        clicked_fn=self._toggle_server,
+                    )
+                    ui.Label(
+                        f"Server: {'RUNNING' if running else 'stopped'}",
+                        width=0,
+                        style={"color": 0xFF33CC33 if running else 0xFF888888},
+                    )
+
                 ui.Label(
                     "\u26a0  Remember to save after each edit",
                     height=0,
@@ -336,6 +348,28 @@ def _print_config(self):
         if self._manager is not None:
             self._manager.scan_and_print()
 
+    def _toggle_server(self):
+        # Start/stop the live server at the click of this button, regardless of the
+        # serverEnabled attribute. Builds from the prim that's actually on the stage.
+        import carb
+
+        if self._manager is None:
+            return
+        if not self._manager.is_running:
+            prim = self._find_interface()
+            if prim is None:
+                carb.log_warn("[natnet] No interface on stage — Create/Save one first.")
+                return
+            cfg = read_interface(prim)
+            self._manager.log_target_diagnostics(cfg)
+            try:
+                self._manager.start_server(cfg)
+            except Exception as exc:  # noqa: BLE001 - surface to the user
+                carb.log_error(f"[natnet] Could not start server: {exc}")
+        else:
+            self._manager.stop_server()
+        self._refresh()
+
     def _create_server(self):
         import carb
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
index a355878bb..d72ba3472 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -113,6 +113,23 @@ def is_interface(prim) -> bool:
     return bool(attr and attr.HasAuthoredValue() and bool(attr.Get()))
 
 
+def resolve_targets(stage, config):
+    """Split a config's bodies into (existing, missing) by target prim presence.
+
+    A body whose ``target_prim`` is empty or points at a non-existent prim lands in
+    ``missing``. Returns two lists of :class:`BodyBinding`.
+    """
+    existing = []
+    missing = []
+    for body in config.bodies:
+        prim = stage.GetPrimAtPath(body.target_prim) if body.target_prim else None
+        if prim is not None and prim.IsValid():
+            existing.append(body)
+        else:
+            missing.append(body)
+    return existing, missing
+
+
 # --- internal helpers ----------------------------------------------------------
 
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
new file mode 100644
index 000000000..407618965
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
@@ -0,0 +1,115 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commit 3 — hermetic catalog-builder tests (no USD, no Kit).
+
+Covers no / single / multiple bodies, field fidelity on the wire, name truncation,
+the MAX_MODELS guard, and duplicate-target detection.
+"""
+
+from __future__ import annotations
+
+import struct
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.catalog import build_catalog, find_duplicate_targets
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig
+from optitrack.natnet.emulator.server import natnet_model_types as mt
+from optitrack.natnet.emulator.server.natnet_common import ModelLimits
+
+pytestmark = pytest.mark.unit
+
+
+def _unpack_bodies(payload: bytes):
+    """Decode a packed sDataDescriptions into [(name, id, parentID), ...]."""
+    (n,) = struct.unpack_from("<i", payload, 0)
+    offset = 4
+    bodies = []
+    for _ in range(n):
+        desc_type, size_bytes = struct.unpack_from("<ii", payload, offset)
+        offset += 8
+        body = payload[offset : offset + size_bytes]
+        offset += size_bytes
+        assert desc_type == int(mt.DataDescriptors.Descriptor_RigidBody)
+        name, rest = body.split(b"\x00", 1)
+        body_id, parent_id = struct.unpack_from("<ii", rest, 0)
+        bodies.append((name, body_id, parent_id))
+    return n, bodies
+
+
+def test_no_bodies_packs_empty_catalog():
+    catalog = build_catalog(NatNetInterfaceConfig())
+    assert catalog.nDataDescriptions == 0
+    n, bodies = _unpack_bodies(catalog.pack())
+    assert n == 0
+    assert bodies == []
+
+
+def test_single_body_field_fidelity():
+    cfg = NatNetInterfaceConfig(
+        bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=7, parent_id=-1)]
+    )
+    catalog = build_catalog(cfg)
+    assert catalog.nDataDescriptions == 1
+    n, bodies = _unpack_bodies(catalog.pack())
+    assert n == 1
+    assert bodies == [(b"Drone", 7, -1)]
+
+
+def test_multiple_bodies_preserve_order_and_fields():
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("Alpha", "/World/a", streaming_id=1, parent_id=-1),
+            BodyBinding("Bravo", "/World/b", streaming_id=2, parent_id=1),
+            BodyBinding("Charlie", "/World/c", streaming_id=5, parent_id=2),
+        ]
+    )
+    catalog = build_catalog(cfg)
+    assert catalog.nDataDescriptions == 3
+    _n, bodies = _unpack_bodies(catalog.pack())
+    assert bodies == [
+        (b"Alpha", 1, -1),
+        (b"Bravo", 2, 1),
+        (b"Charlie", 5, 2),
+    ]
+
+
+def test_long_name_is_truncated_not_rejected():
+    long_name = "X" * 400
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding(long_name, "/World/x", streaming_id=1)])
+    catalog = build_catalog(cfg)
+    _n, bodies = _unpack_bodies(catalog.pack())
+    name = bodies[0][0]
+    assert name == b"X" * (int(ModelLimits.MAX_NAMELENGTH) - 1)
+
+
+def test_too_many_bodies_raises():
+    bodies = [BodyBinding(f"B{i}", f"/World/b{i}", streaming_id=i + 1) for i in range(int(ModelLimits.MAX_MODELS) + 1)]
+    cfg = NatNetInterfaceConfig(bodies=bodies)
+    with pytest.raises(ValueError):
+        build_catalog(cfg)
+
+
+def test_find_duplicate_targets_none():
+    cfg = NatNetInterfaceConfig(
+        bodies=[BodyBinding("A", "/World/a", 1), BodyBinding("B", "/World/b", 2)]
+    )
+    assert find_duplicate_targets(cfg) == []
+
+
+def test_find_duplicate_targets_flags_shared_prim():
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("A", "/World/shared", 1),
+            BodyBinding("B", "/World/shared", 2),
+            BodyBinding("C", "/World/other", 3),
+        ]
+    )
+    assert find_duplicate_targets(cfg) == ["/World/shared"]
+
+
+def test_find_duplicate_targets_ignores_empty_targets():
+    cfg = NatNetInterfaceConfig(
+        bodies=[BodyBinding("A", "", 1), BodyBinding("B", "", 2)]
+    )
+    assert find_duplicate_targets(cfg) == []
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
new file mode 100644
index 000000000..2339cd9fc
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
@@ -0,0 +1,91 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commit 3 — end-to-end: manager builds a real server from a config.
+
+Functional loopback (real UDP sockets, default factory): the manager starts a
+server from a multi-body config, a NatNet client connects, requests MODELDEF, and
+the served catalog must match ``build_catalog(config)``. Then a clean stop must
+free the port so a fresh server can re-bind it.
+"""
+
+from __future__ import annotations
+
+import struct
+import time
+
+import pytest
+
+from natnet_test_helpers import NatNetTestClient, ephemeral_udp_port
+
+from optitrack.natnet.emulator.isaac.catalog import build_catalog
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig
+from optitrack.natnet.emulator.isaac.manager import NatNetServerManager
+from optitrack.natnet.emulator.server import natnet_server_types as st
+
+pytestmark = pytest.mark.unit
+
+
+def _loopback_config(bodies):
+    command_port = ephemeral_udp_port()
+    data_port = ephemeral_udp_port()
+    while data_port == command_port:
+        data_port = ephemeral_udp_port()
+    return NatNetInterfaceConfig(
+        server_ip="127.0.0.1",
+        command_port=command_port,
+        data_port=data_port,
+        bodies=bodies,
+    )
+
+
+def _request_modeldef(command_port):
+    client = NatNetTestClient(timeout=2.0)
+    try:
+        client.send_message(command_port, st.MessageId.NAT_CONNECT)
+        client.recv_message()
+        client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF)
+        message_id, payload, _addr = client.recv_message()
+    finally:
+        client.close()
+    assert message_id == int(st.MessageId.NAT_MODELDEF)
+    return payload
+
+
+def test_server_from_config_serves_matching_catalog():
+    cfg = _loopback_config(
+        [
+            BodyBinding("Alpha", "/World/a", streaming_id=1),
+            BodyBinding("Bravo", "/World/b", streaming_id=2, parent_id=1),
+        ]
+    )
+    mgr = NatNetServerManager()  # default (real) factory
+    assert mgr.start_server(cfg) is True
+    try:
+        time.sleep(0.05)
+        payload = _request_modeldef(cfg.command_port)
+    finally:
+        mgr.stop_server()
+
+    assert payload == build_catalog(cfg).pack()
+    (n,) = struct.unpack_from("<i", payload, 0)
+    assert n == 2
+    assert b"Alpha\x00" in payload
+    assert b"Bravo\x00" in payload
+
+
+def test_stop_frees_port_for_restart():
+    cfg = _loopback_config([BodyBinding("Drone", "/World/base_link", 1)])
+    mgr = NatNetServerManager()
+
+    assert mgr.start_server(cfg) is True
+    mgr.stop_server()
+    time.sleep(0.05)
+
+    # Re-binding the same ports must succeed (clean shutdown released them).
+    assert mgr.start_server(cfg) is True
+    try:
+        time.sleep(0.05)
+        payload = _request_modeldef(cfg.command_port)
+        assert b"Drone\x00" in payload
+    finally:
+        mgr.stop_server()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
new file mode 100644
index 000000000..549903b19
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
@@ -0,0 +1,161 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commits 3-4 — server lifecycle tests with a mocked server factory.
+
+These never bind a socket: a fake factory records construction/start/shutdown so we
+can assert the manager creates and starts the server exactly once, is idempotent,
+toggles, and reconciles to ``serverEnabled``. No USD, no Kit.
+"""
+
+from __future__ import annotations
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig
+from optitrack.natnet.emulator.isaac.manager import (
+    NatNetServerManager,
+    default_server_factory,
+)
+
+pytestmark = pytest.mark.unit
+
+
+class FakeServer:
+    def __init__(self):
+        self.start_calls = 0
+        self.shutdown_calls = 0
+        self.model_def_payload = None
+
+    def set_model_def_payload(self, payload):
+        self.model_def_payload = payload
+
+    def start(self):
+        self.start_calls += 1
+
+    def shutdown(self):
+        self.shutdown_calls += 1
+
+
+class RecordingFactory:
+    """Factory that records every server it builds (one per enable)."""
+
+    def __init__(self):
+        self.created = []
+
+    def __call__(self, config):
+        server = FakeServer()
+        self.created.append(server)
+        return server
+
+
+def _cfg(enabled=True, bodies=None):
+    return NatNetInterfaceConfig(
+        server_ip="127.0.0.1",
+        server_enabled=enabled,
+        bodies=bodies if bodies is not None else [BodyBinding("Drone", "/World/base_link", 1)],
+    )
+
+
+def test_start_creates_and_starts_server_once():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+
+    assert mgr.start_server(_cfg()) is True
+    assert mgr.is_running is True
+    assert len(factory.created) == 1
+    server = factory.created[0]
+    assert server.start_calls == 1
+    assert server.model_def_payload is not None  # catalog was set before start
+
+
+def test_start_is_idempotent_only_one_server():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+
+    assert mgr.start_server(_cfg()) is True
+    # Repeated starts must not build or start a second server.
+    assert mgr.start_server(_cfg()) is False
+    assert mgr.start_server(_cfg()) is False
+
+    assert len(factory.created) == 1
+    assert factory.created[0].start_calls == 1
+
+
+def test_stop_shuts_down_and_is_idempotent():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+
+    mgr.start_server(_cfg())
+    assert mgr.stop_server() is True
+    assert mgr.is_running is False
+    assert factory.created[0].shutdown_calls == 1
+    # Stopping again is a harmless no-op.
+    assert mgr.stop_server() is False
+    assert factory.created[0].shutdown_calls == 1
+
+
+def test_toggle_starts_then_stops():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+
+    assert mgr.toggle_server(_cfg()) is True
+    assert mgr.is_running is True
+    assert mgr.toggle_server(_cfg()) is False
+    assert mgr.is_running is False
+    # A second enable builds a *fresh* server instance.
+    assert mgr.toggle_server(_cfg()) is True
+    assert len(factory.created) == 2
+
+
+def test_apply_enabled_reconciles_to_flag():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+
+    mgr.apply_enabled(_cfg(enabled=False))
+    assert mgr.is_running is False
+    assert len(factory.created) == 0
+
+    mgr.apply_enabled(_cfg(enabled=True))
+    assert mgr.is_running is True
+    assert len(factory.created) == 1
+
+    # Re-applying the same enabled state does not build another server.
+    mgr.apply_enabled(_cfg(enabled=True))
+    assert len(factory.created) == 1
+
+    mgr.apply_enabled(_cfg(enabled=False))
+    assert mgr.is_running is False
+    assert factory.created[0].shutdown_calls == 1
+
+
+def test_on_shutdown_stops_running_server():
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+    mgr.start_server(_cfg())
+    mgr.on_shutdown()
+    assert mgr.is_running is False
+    assert factory.created[0].shutdown_calls == 1
+
+
+@pytest.mark.parametrize("n_bodies", [0, 1, 3])
+def test_start_handles_no_single_and_multiple_bodies(n_bodies):
+    factory = RecordingFactory()
+    mgr = NatNetServerManager(server_factory=factory)
+    bodies = [BodyBinding(f"B{i}", f"/World/b{i}", i + 1) for i in range(n_bodies)]
+    assert mgr.start_server(_cfg(bodies=bodies)) is True
+    assert mgr.is_running is True
+
+
+def test_default_factory_rejects_multicast_without_constructing():
+    cfg = NatNetInterfaceConfig(mode="multicast", multicast_addr="239.255.42.99")
+    with pytest.raises(NotImplementedError):
+        default_server_factory(cfg)
+
+
+def test_default_factory_builds_unicast_server_without_starting():
+    cfg = NatNetInterfaceConfig(server_ip="127.0.0.1", command_port=1610, data_port=1611)
+    server = default_server_factory(cfg)
+    # Constructed but not started: no threads, not running.
+    assert server.running is False
+    assert server.command_port == 1610
+    assert server.data_port == 1611
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
new file mode 100644
index 000000000..b7afa1e91
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
@@ -0,0 +1,90 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Commit 3 — target prim resolution against a real (in-memory) USD stage.
+
+Guarded by ``importorskip("pxr")`` so the suite stays green without usd-core.
+Covers no / single / multiple bodies, missing prims, empty targets, and multiple
+catalog entries pointing at the same prim.
+"""
+
+from __future__ import annotations
+
+import pytest
+
+pytest.importorskip("pxr")
+
+from pxr import Usd, UsdGeom  # noqa: E402
+
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig  # noqa: E402
+from optitrack.natnet.emulator.isaac.usd_bindings import resolve_targets  # noqa: E402
+
+pytestmark = pytest.mark.unit
+
+
+def _stage_with(*paths):
+    stage = Usd.Stage.CreateInMemory()
+    for path in paths:
+        UsdGeom.Xform.Define(stage, path)
+    return stage
+
+
+def _names(bodies):
+    return [b.rigid_body_name for b in bodies]
+
+
+def test_no_bodies_resolves_empty():
+    stage = _stage_with("/World/base_link")
+    existing, missing = resolve_targets(stage, NatNetInterfaceConfig())
+    assert existing == []
+    assert missing == []
+
+
+def test_single_existing_target():
+    stage = _stage_with("/World/base_link")
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "/World/base_link", 1)])
+    existing, missing = resolve_targets(stage, cfg)
+    assert _names(existing) == ["Drone"]
+    assert missing == []
+
+
+def test_missing_target_is_reported():
+    stage = _stage_with("/World/base_link")
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Ghost", "/World/does_not_exist", 1)])
+    existing, missing = resolve_targets(stage, cfg)
+    assert existing == []
+    assert _names(missing) == ["Ghost"]
+
+
+def test_empty_target_is_missing():
+    stage = _stage_with("/World/base_link")
+    cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Unpointed", "", 1)])
+    existing, missing = resolve_targets(stage, cfg)
+    assert existing == []
+    assert _names(missing) == ["Unpointed"]
+
+
+def test_mixed_existing_and_missing():
+    stage = _stage_with("/World/a", "/World/c")
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("A", "/World/a", 1),
+            BodyBinding("B", "/World/b", 2),  # missing
+            BodyBinding("C", "/World/c", 3),
+        ]
+    )
+    existing, missing = resolve_targets(stage, cfg)
+    assert _names(existing) == ["A", "C"]
+    assert _names(missing) == ["B"]
+
+
+def test_multiple_entries_same_prim_all_resolve():
+    stage = _stage_with("/World/shared")
+    cfg = NatNetInterfaceConfig(
+        bodies=[
+            BodyBinding("First", "/World/shared", 1),
+            BodyBinding("Second", "/World/shared", 2),
+        ]
+    )
+    existing, missing = resolve_targets(stage, cfg)
+    assert _names(existing) == ["First", "Second"]
+    assert missing == []
diff --git a/tests/sim/optitrack_natnet_emulator/test_catalog.py b/tests/sim/optitrack_natnet_emulator/test_catalog.py
new file mode 100644
index 000000000..0d19bf34b
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_catalog.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 catalog-builder tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_catalog.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_server_from_config.py b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py
new file mode 100644
index 000000000..f84c2e2a0
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 server-from-config tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_server_from_config.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
new file mode 100644
index 000000000..9cf4a45d6
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator Commits 3-4 lifecycle unit tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_server_lifecycle.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_target_resolution.py b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py
new file mode 100644
index 000000000..380b60e9b
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 target-resolution tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_target_resolution.py",
+)

From 846ba70fc4ae418656276d0be316c442c1594ab9 Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Thu, 11 Jun 2026 11:58:44 -0400
Subject: [PATCH 17/27] working single agent case

---
 .agents/skills/optitrack-development/SKILL.md |   2 +
 .../src/perception/natnet_ros2/README.md      |   4 +
 .../natnet_ros2/config/natnet_config.yaml     |   4 +-
 .../natnet_ros2/src/natnet_ros2_node.cpp      |  44 +++-
 .../isaac-sim/docker/docker-compose.yaml      |   5 +
 .../natnet/emulator/isaac/__init__.py         |  14 ++
 .../optitrack/natnet/emulator/isaac/frames.py |  81 +++++++
 .../natnet/emulator/isaac/manager.py          | 145 ++++++++++++-
 .../natnet/emulator/isaac/scene_setup.py      | 103 +++++++++
 .../natnet/emulator/isaac/ui_extension.py     | 139 +++++++++---
 .../natnet/emulator/isaac/usd_bindings.py     |  28 +++
 .../natnet/emulator/server/natnet_server.py   |   7 +
 .../emulator/server/natnet_unicast_server.py  |  44 ++--
 .../test/test_frames.py                       |  78 +++++++
 .../test/test_pose_sampling.py                | 202 ++++++++++++++++++
 .../test/test_pose_streaming.py               |  87 ++++++++
 .../test/test_scene_setup.py                  |  87 ++++++++
 ...example_multi_px4_pegasus_launch_script.py |  43 ++++
 .../example_one_px4_pegasus_launch_script.py  |  34 +++
 tests/docker/docker-compose.yaml              |   5 +
 tests/integration/natnet/README.md            |  37 +++-
 .../natnet/test_natnet_integration.py         | 197 ++++++++++++-----
 .../optitrack_natnet_emulator/test_frames.py  |  11 +
 .../test_pose_sampling.py                     |  11 +
 .../test_pose_streaming.py                    |  11 +
 .../test_scene_setup.py                       |  11 +
 tests/system/test_liveliness.py               |  60 ++++++
 27 files changed, 1373 insertions(+), 121 deletions(-)
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
 create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_frames.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
 create mode 100644 tests/sim/optitrack_natnet_emulator/test_scene_setup.py

diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md
index 0aa8fbc04..4120510c4 100644
--- a/.agents/skills/optitrack-development/SKILL.md
+++ b/.agents/skills/optitrack-development/SKILL.md
@@ -43,6 +43,8 @@ flowchart LR
 
 **Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`.
 
+**Enable in sim:** the same `LAUNCH_NATNET=true` (passed into the isaac-sim container via [`docker-compose.yaml`](../../../simulation/isaac-sim/docker/docker-compose.yaml)) makes the Pegasus example launch scripts author a `/World/NatNetInterface` prim with one rigid body per drone `base_link` and **auto-start the emulator on load** (no UI). Helper: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`). Body name: single = `Drone`; multi = `Drone<i>` (id `i`). Override the name prefix with `NATNET_BODY_NAME`.
+
 **Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511 — see [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
 
 ## NatNet: Two UDP Channels
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index eb44763b4..502525071 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -147,6 +147,10 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`:
 - Invalid/malformed packets are skipped with debug logging
 - Lost connectivity logs warnings; gracefully recovers when stream resumes
 - Covariance in config allows tuning uncertainty per deployment
+- **Connect retry:** the initial handshake is retried every 2 s until it
+  succeeds, so the node tolerates the NatNet server starting *after* the robot
+  (e.g. a Motive PC powered on later, or the Isaac Sim NatNet emulator which only
+  binds ~100 s into sim boot). The retry timer cancels itself on first success.
 
 ## Testing
 
diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
index 69fc11d1c..dc0b3a0e0 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
+++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
@@ -7,8 +7,8 @@
 /**:
   ros__parameters:
     # IP address of the PC running Motive (OptiTrack server).
-    # Change this to match your local network before launching NatNet.
-    server_ip: "192.168.1.100"
+    # Defaults to the Isaac Sim container on the airstack_network (172.31.0.200),
+    server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)"
     # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have
     # multiple NICs (e.g. Docker 172.17.* vs LAN).
     client_ip: "0.0.0.0"
diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
index 65059f659..7e6044ac3 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
+++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
@@ -117,7 +117,18 @@ class NatNetROS2Node : public rclcpp::Node
 
         // Production client — NatNetClientAdapter wraps the SDK
         client_ = std::make_unique<natnet_ros2::NatNetClientAdapter>();
-        connect_and_setup(connect_cfg);
+        connect_cfg_ = connect_cfg;
+
+        // Try to connect now; if the server is not up yet keep retrying. The
+        // NatNet server may legitimately start *after* the robot — e.g. the Isaac
+        // Sim emulator only binds ~100 s into sim boot, and a real Motive PC may be
+        // powered on after the drone. A one-shot connect would leave us dead for
+        // the whole session, so retry on a timer until the first handshake lands.
+        if (!connect_and_setup(connect_cfg_)) {
+            connect_timer_ = this->create_wall_timer(
+                std::chrono::seconds(2),
+                std::bind(&NatNetROS2Node::retry_connect, this));
+        }
 
         refresh_timer_ = this->create_wall_timer(
             std::chrono::seconds(1),
@@ -200,14 +211,16 @@ class NatNetROS2Node : public rclcpp::Node
 
 private:
     // -----------------------------------------------------------------------
-    void connect_and_setup(const natnet_ros2::ConnectConfig & cfg)
+    // Returns true once the handshake succeeds and the frame callback is live.
+    // On failure it logs (WARN) and returns false so the caller can retry.
+    bool connect_and_setup(const natnet_ros2::ConnectConfig & cfg)
     {
         const natnet_ros2::NegotiationResult neg =
             natnet_ros2::negotiate(*client_, cfg);
 
         if (!neg.ok) {
-            RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str());
-            return;
+            RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str());
+            return false;
         }
 
         if (neg.server_info.host_present) {
@@ -221,6 +234,26 @@ class NatNetROS2Node : public rclcpp::Node
         client_->set_frame_callback(
             [this](const natnet_ros2::FrameSample & f) { on_frame(f); });
         RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data.");
+        connected_ = true;
+        return true;
+    }
+
+    // -----------------------------------------------------------------------
+    // Timer-driven reconnect: fires every 2 s until the first handshake lands,
+    // then cancels itself. Runs on the node's executor thread (same as the
+    // refresh timer), so no extra locking versus single-threaded init.
+    void retry_connect()
+    {
+        if (connected_) {
+            if (connect_timer_) { connect_timer_->cancel(); }
+            return;
+        }
+        RCLCPP_INFO(get_logger(),
+            "NatNet not connected — retrying handshake to %s ...",
+            connect_cfg_.server_ip.c_str());
+        if (connect_and_setup(connect_cfg_) && connect_timer_) {
+            connect_timer_->cancel();
+        }
     }
 
     // -----------------------------------------------------------------------
@@ -304,6 +337,8 @@ class NatNetROS2Node : public rclcpp::Node
     std::array<double, 36> covariance_6x6_{};
 
     std::unique_ptr<natnet_ros2::INatNetClient> client_;
+    natnet_ros2::ConnectConfig connect_cfg_;
+    bool connected_ = false;
 
     std::mutex pub_mutex_;
     std::unordered_map<int32_t, std::string>    body_names_;
@@ -311,6 +346,7 @@ class NatNetROS2Node : public rclcpp::Node
 
     std::atomic<bool> needs_description_refresh_{false};
     rclcpp::TimerBase::SharedPtr refresh_timer_;
+    rclcpp::TimerBase::SharedPtr connect_timer_;
 };
 
 
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index de2ff5ae5..c37e8cb66 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -42,6 +42,11 @@ services:
       - NUM_ROBOTS=${NUM_ROBOTS:-1}
       - ENABLE_LIDAR=${ENABLE_LIDAR:-false}
       - ISAAC_SIM_HEADLESS=${ISAAC_SIM_HEADLESS:-false}
+      # OptiTrack/NatNet emulator: mirror the robot-side LAUNCH_NATNET gate so the
+      # example launch scripts author a NatNet interface (one rigid body per drone
+      # base_link) and start the server on load when enabled.
+      - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
+      - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone}
       # Pegasus physics tuning — read by pegasus/simulator/params.py
       - PX4_PHYSICS_HZ=${PX4_PHYSICS_HZ:-100}
       - ARDUPILOT_PHYSICS_HZ=${ARDUPILOT_PHYSICS_HZ:-800}
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
index 4bc914951..9bb35e108 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -14,28 +14,42 @@
     make_instance_key,
 )
 from .catalog import build_catalog, find_duplicate_targets
+from .frames import BodySample, build_frame, make_rigid_body_data
 from .manager import NatNetServerManager, default_server_factory, format_interface
+from .scene_setup import (
+    DEFAULT_INTERFACE_PATH,
+    build_drone_config,
+    start_drone_natnet_server,
+)
 from .usd_bindings import (
     author_interface,
     find_interfaces,
     is_interface,
     read_interface,
+    read_world_pose,
     resolve_targets,
 )
 
 __all__ = [
+    "DEFAULT_INTERFACE_PATH",
     "BodyBinding",
+    "BodySample",
     "NatNetInterfaceConfig",
     "NatNetServerManager",
     "author_interface",
     "body_attr_name",
     "build_catalog",
+    "build_drone_config",
+    "build_frame",
     "default_server_factory",
     "find_duplicate_targets",
     "find_interfaces",
     "format_interface",
     "is_interface",
     "make_instance_key",
+    "make_rigid_body_data",
     "read_interface",
+    "read_world_pose",
     "resolve_targets",
+    "start_drone_natnet_server",
 ]
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
new file mode 100644
index 000000000..4902fb93c
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
@@ -0,0 +1,81 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Pose -> NatNet frame conversion (the data-enqueue path).
+
+Pure Python + ctypes (no USD, no Kit), so it's hermetically unit-testable. Sampled
+prim world poses become an ``sFrameOfMocapData`` of rigid bodies that the server
+streams to ``natnet_ros2``.
+
+**Frame convention:** AirStack's ``natnet_ros2`` copies the rigid-body pose straight
+through (``rb_to_pose`` is an identity copy) and nothing downstream re-axes it, so we
+emit the prim's USD world pose **as-is** (no Z-up->Y-up swap). Swapping here would
+desync the published pose from the rest of the stack.
+
+**params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``):
+``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it.
+``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF
+(set the frame after the catalog changes, e.g. a body added live).
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+from ..server.natnet_data_types import sFrameOfMocapData, sRigidBodyData
+
+TRACKING_VALID = 0x01
+MODEL_LIST_CHANGED = 0x02
+
+
+@dataclass
+class BodySample:
+    """One sampled rigid body: streaming ID + world pose, or an invalid (lost) body."""
+
+    streaming_id: int
+    position: tuple[float, float, float] = (0.0, 0.0, 0.0)
+    orientation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)  # qx,qy,qz,qw
+    valid: bool = True
+
+    @classmethod
+    def lost(cls, streaming_id: int) -> "BodySample":
+        """An untracked body (missing prim): NaN position, tracking-invalid bit clear."""
+        nan = float("nan")
+        return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False)
+
+
+def make_rigid_body_data(sample: BodySample) -> sRigidBodyData:
+    """Build one ``sRigidBodyData`` from a sample (sets the tracking-valid bit)."""
+    rb = sRigidBodyData()
+    rb.ID = int(sample.streaming_id)
+    x, y, z = sample.position
+    qx, qy, qz, qw = sample.orientation
+    rb.x, rb.y, rb.z = float(x), float(y), float(z)
+    rb.qx, rb.qy, rb.qz, rb.qw = float(qx), float(qy), float(qz), float(qw)
+    rb.MeanError = 0.0
+    rb.params = TRACKING_VALID if sample.valid else 0
+    return rb
+
+
+def build_frame(
+    frame_number: int,
+    samples,
+    *,
+    timestamp: float = 0.0,
+    model_list_changed: bool = False,
+) -> sFrameOfMocapData:
+    """Assemble an ``sFrameOfMocapData`` of rigid bodies from samples."""
+    frame = sFrameOfMocapData()
+    frame.iFrame = int(frame_number)
+    samples = list(samples)
+    frame.nRigidBodies = len(samples)
+    for i, sample in enumerate(samples):
+        frame.RigidBodies[i] = make_rigid_body_data(sample)
+    frame.fTimestamp = float(timestamp)
+    frame.params = MODEL_LIST_CHANGED if model_list_changed else 0
+    return frame
+
+
+def is_finite_pose(sample: BodySample) -> bool:
+    """True if all position/orientation components are finite (no NaN/inf)."""
+    return all(math.isfinite(v) for v in (*sample.position, *sample.orientation))
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
index e473e6c35..197ed3b2a 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -18,7 +18,13 @@
 
 from .catalog import build_catalog, find_duplicate_targets
 from .config import NatNetInterfaceConfig
-from .usd_bindings import find_interfaces, read_interface, resolve_targets
+from .frames import BodySample, build_frame
+from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets
+
+
+def _catalog_signature(config: NatNetInterfaceConfig):
+    """Identity of the catalog (body id/name set) — changes trigger a MODELDEF refresh."""
+    return tuple((b.streaming_id, b.rigid_body_name) for b in config.bodies)
 
 
 def _parse_version(version_str: str) -> tuple[int, int, int, int]:
@@ -83,6 +89,15 @@ def __init__(self, server_factory=None):
         self._scan_pending = False
         self._server = None
         self._server_factory = server_factory or default_server_factory
+        # Sampling state. ``_needs_resync`` is the "latest config has been read"
+        # flag inverted: a NatNet prim edit sets it True (stale); the next physics
+        # sample re-reads the catalog/targets and clears it. ``_sample_cache`` holds
+        # the resolved (streaming_id, name, prim) tuples sampled every step.
+        self._needs_resync = False
+        self._sample_cache: list = []
+        self._frame_counter = 0
+        self._catalog_signature = None
+        self._physx_sub = None
 
     # --- lifecycle -------------------------------------------------------------
 
@@ -94,16 +109,34 @@ def on_startup(self):
             self._on_stage_event, name="natnet_manager_stage_events"
         )
         self._register_usd_listener()
+        self._subscribe_physics()
         print("[natnet] NatNetServerManager initialized")
         self.scan_and_print()
 
     def on_shutdown(self):
         self.stop_server()
+        self._physx_sub = None
         self._stage_event_sub = None
         self._scan_tick_sub = None
         self._scan_pending = False
         self._revoke_usd_listener()
 
+    def _subscribe_physics(self):
+        # Sample + enqueue poses on every physics step (only fires while playing).
+        try:
+            import omni.physx
+
+            self._physx_sub = omni.physx.get_physx_interface().subscribe_physics_step_events(
+                self._on_physics_step
+            )
+        except Exception as exc:  # pragma: no cover - Kit/physx only
+            print(f"[natnet] Physics step subscription unavailable: {exc}")
+            self._physx_sub = None
+
+    def _on_physics_step(self, _dt):
+        if self._server is not None:
+            self.sample_once()
+
     # --- scanning --------------------------------------------------------------
 
     def scan_and_print(self, *_):
@@ -142,8 +175,20 @@ def start_server(self, config: NatNetInterfaceConfig) -> bool:
         catalog = build_catalog(config)
         server = self._server_factory(config)
         server.set_model_def_payload(catalog.pack())
+        # Pump frames from our sample_once (physics-step) thread rather than the
+        # server's background timer: inside the Isaac Sim process that daemon thread
+        # is starved by the render/physics main loop, so frames never get sent.
+        if hasattr(server, "auto_stream"):
+            server.auto_stream = False
         server.start()
         self._server = server
+        # Force a resync on the first sampled frame so the prim->pose cache is built
+        # from the live stage (and the catalog signature is seeded).
+        self._needs_resync = True
+        self._frame_counter = 0
+        # None so the first resync reports "changed" and the first streamed frame
+        # flags model_list_changed (nudging the client to (re)read MODELDEF).
+        self._catalog_signature = None
         print(
             f"[natnet] Server started on {config.server_ip} "
             f"(cmd {config.command_port} / data {config.data_port}) "
@@ -162,6 +207,8 @@ def stop_server(self) -> bool:
             self._server.shutdown()
         finally:
             self._server = None
+            self._sample_cache = []
+            self._needs_resync = False
         print("[natnet] Server stopped.")
         return True
 
@@ -193,6 +240,98 @@ def log_target_diagnostics(self, config: NatNetInterfaceConfig) -> None:
         for path in find_duplicate_targets(config):
             print(f"[natnet] WARNING: multiple bodies target the same prim: {path}")
 
+    # --- scripting entry point -------------------------------------------------
+
+    def start_from_stage(self) -> bool:
+        """Find the interface prim on the current stage, read it, and start.
+
+        Convenience for scripts/Pegasus launchers: author the prim (see
+        ``author_interface``) then call this. Returns False if nothing to start.
+        """
+        stage = self._get_stage()
+        if stage is None:
+            print("[natnet] start_from_stage: no active stage.")
+            return False
+        interfaces = find_interfaces(stage)
+        if not interfaces:
+            print("[natnet] start_from_stage: no NatNetInterface prim found.")
+            return False
+        config = read_interface(interfaces[0])
+        self.log_target_diagnostics(config)
+        return self.start_server(config)
+
+    # --- pose sampling + dynamic catalog (the data-enqueue path) ---------------
+
+    def mark_dirty(self) -> None:
+        """Flag that the on-stage config changed; next sample re-reads the catalog."""
+        self._needs_resync = True
+
+    def _resync(self, stage) -> bool:
+        """Re-read the interface config, rebuild the catalog, and re-resolve targets.
+
+        Returns True if the catalog (body id/name set) actually changed, so the next
+        frame can flag ``model_list_changed`` and the client re-requests MODELDEF.
+        """
+        interfaces = find_interfaces(stage)
+        if not interfaces:
+            self._sample_cache = []
+            return False
+        config = read_interface(interfaces[0])
+        if self._server is not None:
+            self._server.set_model_def_payload(build_catalog(config).pack())
+        # Cache target *paths* (not prim handles): the prim is re-resolved every
+        # sample so bodies whose target is created *after* the server starts — e.g.
+        # a Pegasus drone base_link spawned on the first Play tick — start streaming
+        # a valid pose as soon as the prim appears (instead of being stuck "lost").
+        self._sample_cache = [
+            (body.streaming_id, body.rigid_body_name, body.target_prim)
+            for body in config.bodies
+        ]
+        signature = _catalog_signature(config)
+        changed = signature != self._catalog_signature
+        self._catalog_signature = signature
+        return changed
+
+    def sample_once(self, stage=None):
+        """Sample every body's USD world pose and enqueue one frame to the server.
+
+        Resyncs the catalog first if the config is dirty (so bodies added/removed
+        live are picked up). Returns the enqueued frame (or None if nothing to do).
+        """
+        if self._server is None:
+            return None
+        if stage is None:
+            stage = self._get_stage()
+        if stage is None:
+            return None
+
+        model_changed = False
+        if self._needs_resync:
+            model_changed = self._resync(stage)
+            self._needs_resync = False
+
+        samples = []
+        for streaming_id, _name, target_path in self._sample_cache:
+            prim = stage.GetPrimAtPath(target_path) if target_path else None
+            pose = read_world_pose(prim) if prim is not None else None
+            if pose is None:
+                samples.append(BodySample.lost(streaming_id))
+            else:
+                position, orientation = pose
+                samples.append(BodySample(streaming_id, position, orientation, valid=True))
+
+        frame = build_frame(
+            self._frame_counter, samples, model_list_changed=model_changed
+        )
+        self._frame_counter += 1
+        self._server.enqueue_mocap_data(frame)
+        # Send synchronously from this (physics-step) thread; the server's background
+        # data thread is unreliable inside the GIL-bound Isaac Sim process.
+        pump = getattr(self._server, "pump_once", None)
+        if callable(pump):
+            pump()
+        return frame
+
     # --- stage / USD notifications --------------------------------------------
 
     def _get_stage(self):
@@ -231,6 +370,10 @@ def _on_objects_changed(self, notice, sender):
         except Exception:  # pragma: no cover - defensive
             paths = []
         if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths):
+            # A NatNet prim changed (e.g. a body added/retargeted while live): mark
+            # the sampler dirty so the next physics step re-reads the catalog and
+            # nudges the client to refresh MODELDEF.
+            self._needs_resync = True
             # A single author_interface() (Create/Save) emits many notices — one per
             # attribute/relationship op. Debounce them into one scan on the next
             # update tick so we print the final state once, not once per op.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
new file mode 100644
index 000000000..b5672a87b
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
@@ -0,0 +1,103 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Standalone-launch helpers: stand up a drone NatNet interface on scene load.
+
+Used by the Pegasus example launch scripts so a Motive-compatible NatNet server
+comes up automatically with one rigid body per drone ``base_link`` — no UI clicks.
+
+Two layers, mirroring the rest of the package:
+
+- ``build_drone_config`` is **pure** (no USD / Kit), so it unit-tests hermetically.
+- ``start_drone_natnet_server`` authors the interface prim and owns a
+  :class:`~optitrack.natnet.emulator.isaac.manager.NatNetServerManager` that samples
+  poses on each physics step. It imports ``pxr``/``omni`` lazily (only when called).
+"""
+
+from __future__ import annotations
+
+from typing import Iterable, Sequence, Tuple
+
+from .config import (
+    DEFAULT_COMMAND_PORT,
+    DEFAULT_DATA_PORT,
+    DEFAULT_PUBLISH_RATE,
+    DEFAULT_SERVER_IP,
+    BodyBinding,
+    NatNetInterfaceConfig,
+)
+
+# Where the example scripts author the single interface prim.
+DEFAULT_INTERFACE_PATH = "/World/NatNetInterface"
+
+# (rigid_body_name, streaming_id, target_prim_path)
+DroneSpec = Tuple[str, int, str]
+
+
+def build_drone_config(
+    drones: Iterable[DroneSpec],
+    *,
+    server_ip: str = DEFAULT_SERVER_IP,
+    mode: str = "unicast",
+    command_port: int = DEFAULT_COMMAND_PORT,
+    data_port: int = DEFAULT_DATA_PORT,
+    publish_rate: float = DEFAULT_PUBLISH_RATE,
+    server_enabled: bool = True,
+) -> NatNetInterfaceConfig:
+    """Build a validated config with one rigid body per drone.
+
+    ``drones`` is an iterable of ``(rigid_body_name, streaming_id, target_prim)``
+    tuples — typically one per spawned drone, with ``target_prim`` pointing at the
+    drone's ``base_link``. Raises ``ValueError`` (via ``validate``) on duplicate
+    names/ids or bad ports.
+    """
+    bodies = [
+        BodyBinding(
+            rigid_body_name=str(name),
+            target_prim=str(target),
+            streaming_id=int(streaming_id),
+        )
+        for name, streaming_id, target in drones
+    ]
+    cfg = NatNetInterfaceConfig(
+        server_enabled=server_enabled,
+        server_ip=server_ip,
+        mode=mode,
+        command_port=command_port,
+        data_port=data_port,
+        publish_rate=publish_rate,
+        bodies=bodies,
+    )
+    cfg.validate()
+    return cfg
+
+
+def start_drone_natnet_server(
+    stage,
+    drones: Sequence[DroneSpec],
+    *,
+    prim_path: str = DEFAULT_INTERFACE_PATH,
+    start: bool = True,
+    **config_kwargs,
+):
+    """Author a NatNet interface prim from ``drones`` and return a running manager.
+
+    Authors ``prim_path`` (overwriting any existing interface) with one rigid body
+    per drone, then creates a :class:`NatNetServerManager` that subscribes to physics
+    steps and starts the server. Pump the sim (``timeline.play()`` / ``world.step``)
+    to stream poses. **Keep a reference to the returned manager** so it isn't
+    garbage-collected (which would tear down the physics subscription and server).
+
+    Returns the ``NatNetServerManager``. If ``start`` is False (or the config is
+    authored disabled), the manager is created but the server is left stopped.
+    """
+    from .manager import NatNetServerManager
+    from .usd_bindings import author_interface
+
+    cfg = build_drone_config(drones, **config_kwargs)
+    author_interface(stage, prim_path, cfg)
+
+    manager = NatNetServerManager()
+    manager.on_startup()
+    if start and cfg.server_enabled:
+        manager.start_from_stage()
+    return manager
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index afe84e553..246670cac 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -9,9 +9,14 @@
 
 * **Load from Stage** — read the prim into the window (or reset to defaults if none).
 * **Save** — write the window's current values to the prim (validate + author).
-* **Create Server** — ensure the interface prim exists (authored from the current
-  values) and select it. (Actually starting the UDP server arrives in a later commit;
-  for now this creates/initializes the interface prim that the server will read.)
+* **Create Interface** — ensure the interface prim exists (authored from the current
+  values) and select it.
+* **Start/Stop Server** — start/stop the live UDP server directly (the button is the
+  authority, independent of ``serverEnabled``). While running, poses are sampled from
+  the USD stage on every physics step and streamed to ``natnet_ros2``.
+
+Each tracked-body row shows a live readout (status dot + world position pulled from
+the USD stage) so you can see at a glance what each body is publishing.
 
 Edits in the window mutate an in-memory working copy only; nothing touches the prim
 until you Save / Create, so you are in charge of keeping things synced.
@@ -27,10 +32,15 @@
 
 from .config import VALID_MODES, BodyBinding, NatNetInterfaceConfig
 from .manager import NatNetServerManager
-from .usd_bindings import author_interface, find_interfaces, read_interface
+from .usd_bindings import author_interface, find_interfaces, read_interface, read_world_pose
 
 _DEFAULT_PRIM_PATH = "/World/NatNetInterface"
 _LABEL_WIDTH = 140
+_POS_REFRESH_PERIOD = 1.0 / 6.0  # seconds between live USD position reads
+
+_COLOR_LIVE = 0xFF33CC33  # green: prim resolves and server is streaming
+_COLOR_IDLE = 0xFFAAAAAA  # grey: prim resolves but server not running
+_COLOR_LOST = 0xFF3333FF  # red: no prim / NaN
 
 
 class NatNetEmulatorExtension(omni.ext.IExt):
@@ -40,12 +50,18 @@ def on_startup(self, ext_id):  # noqa: D401 - Kit lifecycle hook
         self._window = None
         self._bodies_frame = None
         self._cfg = NatNetInterfaceConfig()
+        self._row_readouts = {}
+        self._pos_refresh_sub = None
+        self._last_pos_refresh = 0.0
         self._manager = NatNetServerManager()
         self._manager.on_startup()
         self._add_menu()
+        self._subscribe_position_refresh()
 
     def on_shutdown(self):
         self._remove_menu()
+        self._pos_refresh_sub = None
+        self._row_readouts = {}
         if self._manager is not None:
             self._manager.on_shutdown()
             self._manager = None
@@ -53,6 +69,53 @@ def on_shutdown(self):
             self._window.destroy()
             self._window = None
 
+    # --- live position readout -------------------------------------------------
+
+    def _subscribe_position_refresh(self):
+        try:
+            import omni.kit.app
+        except Exception:  # pragma: no cover - Kit only
+            return
+        self._pos_refresh_sub = (
+            omni.kit.app.get_app()
+            .get_update_event_stream()
+            .create_subscription_to_pop(self._on_pos_refresh, name="natnet_ui_pos_refresh")
+        )
+
+    def _on_pos_refresh(self, _event):
+        import time
+
+        if self._window is None or not self._window.visible or not self._row_readouts:
+            return
+        now = time.monotonic()
+        if now - self._last_pos_refresh < _POS_REFRESH_PERIOD:
+            return
+        self._last_pos_refresh = now
+        stage = self._get_stage()
+        running = self._manager is not None and self._manager.is_running
+        for idx, (status_label, pos_label) in list(self._row_readouts.items()):
+            if not (0 <= idx < len(self._cfg.bodies)):
+                continue
+            target = self._cfg.bodies[idx].target_prim
+            symbol, color, text = self._row_readout(stage, target, running)
+            status_label.text = symbol
+            status_label.style = {"color": color}
+            pos_label.text = text
+            pos_label.style = {"color": color}
+
+    def _row_readout(self, stage, target, running):
+        if not target:
+            return "\u25cb", _COLOR_IDLE, "no target prim"
+        prim = stage.GetPrimAtPath(target) if stage is not None else None
+        pose = read_world_pose(prim) if prim is not None else None
+        if pose is None:
+            return "\u2717", _COLOR_LOST, "NaN (prim missing)"
+        (x, y, z), _quat = pose
+        text = f"{x:+.3f}, {y:+.3f}, {z:+.3f}"
+        if running:
+            return "\u25cf", _COLOR_LIVE, text
+        return "\u25cf", _COLOR_IDLE, text
+
     # --- menu ------------------------------------------------------------------
 
     def _add_menu(self):
@@ -207,7 +270,8 @@ def _rebuild_bodies(self, *_):
     def _build_bodies(self):
         import omni.ui as ui
 
-        with ui.VStack(spacing=4, height=0):
+        self._row_readouts = {}
+        with ui.VStack(spacing=6, height=0):
             if not self._cfg.bodies:
                 ui.Label("  (no bodies — select a prim and click Add body)", height=0)
                 return
@@ -221,33 +285,45 @@ def _build_bodies(self):
                 self._build_body_row(ui, idx, body)
 
     def _build_body_row(self, ui, idx, body):
-        with ui.HStack(height=0, spacing=4):
-            name = ui.StringField(width=ui.Fraction(1)).model
-            name.set_value(body.rigid_body_name)
-            name.add_value_changed_fn(
-                lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string())
-            )
+        with ui.VStack(height=0, spacing=2):
+            with ui.HStack(height=0, spacing=4):
+                name = ui.StringField(width=ui.Fraction(1)).model
+                name.set_value(body.rigid_body_name)
+                name.add_value_changed_fn(
+                    lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string())
+                )
 
-            sid = ui.IntField(width=40).model
-            sid.set_value(body.streaming_id)
-            sid.add_value_changed_fn(
-                lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int())
-            )
+                sid = ui.IntField(width=40).model
+                sid.set_value(body.streaming_id)
+                sid.add_value_changed_fn(
+                    lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int())
+                )
 
-            parent = ui.IntField(width=50).model
-            parent.set_value(body.parent_id)
-            parent.add_value_changed_fn(
-                lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int())
-            )
+                parent = ui.IntField(width=50).model
+                parent.set_value(body.parent_id)
+                parent.add_value_changed_fn(
+                    lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int())
+                )
 
-            target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model
-            target.set_value(body.target_prim)
-            target.add_value_changed_fn(
-                lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string())
-            )
+                target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model
+                target.set_value(body.target_prim)
+                target.add_value_changed_fn(
+                    lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string())
+                )
+
+                ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i))
+                ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i))
 
-            ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i))
-            ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i))
+            # Live readout: status dot + world position pulled from the USD stage.
+            stage = self._get_stage()
+            running = self._manager is not None and self._manager.is_running
+            symbol, color, text = self._row_readout(stage, body.target_prim, running)
+            with ui.HStack(height=0, spacing=6):
+                ui.Spacer(width=4)
+                status_label = ui.Label(symbol, width=14, style={"color": color})
+                ui.Label("pos:", width=30, style={"color": _COLOR_IDLE})
+                pos_label = ui.Label(text, width=ui.Fraction(1), style={"color": color})
+            self._row_readouts[idx] = (status_label, pos_label)
 
     def _set_body_field(self, index, attr, value):
         if 0 <= index < len(self._cfg.bodies):
@@ -356,14 +432,11 @@ def _toggle_server(self):
         if self._manager is None:
             return
         if not self._manager.is_running:
-            prim = self._find_interface()
-            if prim is None:
+            if self._find_interface() is None:
                 carb.log_warn("[natnet] No interface on stage — Create/Save one first.")
                 return
-            cfg = read_interface(prim)
-            self._manager.log_target_diagnostics(cfg)
             try:
-                self._manager.start_server(cfg)
+                self._manager.start_from_stage()
             except Exception as exc:  # noqa: BLE001 - surface to the user
                 carb.log_error(f"[natnet] Could not start server: {exc}")
         else:
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
index d72ba3472..7fd193c80 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -113,6 +113,34 @@ def is_interface(prim) -> bool:
     return bool(attr and attr.HasAuthoredValue() and bool(attr.Get()))
 
 
+def read_world_pose(prim):
+    """Return ``((x, y, z), (qx, qy, qz, qw))`` from a prim's USD world transform.
+
+    Reads the position/orientation **stored in the USD stage** (the local-to-world
+    transform), which is what the physics step writes back each frame. Returns
+    ``None`` for an invalid/non-xformable prim so callers can mark the body lost.
+    """
+    from pxr import Usd, UsdGeom
+
+    if prim is None or not prim.IsValid():
+        return None
+    xformable = UsdGeom.Xformable(prim)
+    if not xformable:
+        return None
+    matrix = xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default())
+    translation = matrix.ExtractTranslation()
+    quat = matrix.ExtractRotationQuat()  # Gf.Quatd, normalized
+    imaginary = quat.GetImaginary()
+    position = (float(translation[0]), float(translation[1]), float(translation[2]))
+    orientation = (
+        float(imaginary[0]),
+        float(imaginary[1]),
+        float(imaginary[2]),
+        float(quat.GetReal()),
+    )
+    return position, orientation
+
+
 def resolve_targets(stage, config):
     """Split a config's bodies into (existing, missing) by target prim presence.
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
index 4a186e84f..1c1503b3a 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
@@ -85,6 +85,13 @@ def __init__(self,
 
         self.running = False
 
+        # When True (default), the background data loop streams frames on its own
+        # timer. Set False when an external driver (the Isaac wrapper's physics-step
+        # callback) pumps frames synchronously via ``pump_once`` — inside the Isaac
+        # Sim process the daemon send thread is starved by the render/physics main
+        # loop holding the GIL, so frames must be sent from the callback thread.
+        self.auto_stream = True
+
         # start() launches two daemon threads: a command listener (handshake /
         # MODELDEF / keepalive) and a data loop that streams mocap frames. The
         # transmission-specific behavior lives in the unicast/multicast subclass.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index f6da0905f..fdef63b7f 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -20,27 +20,39 @@ def __init__(self,
         super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port)
 
     def _data_update_loop(self):
-        # Loop to update mocap data and send packets at regular intervals
+        # Loop to update mocap data and send packets at regular intervals. When
+        # auto_stream is False the frames are pumped externally (Isaac physics step),
+        # so this thread only idles — but stays alive for clean shutdown.
         while not self.shutdown_event.is_set():
             time.sleep(1 / self.publish_rate)
-
-            with self.clients_lock:
-                clients = list(self.connected_clients)
-            if not clients:
+            if not self.auto_stream:
                 continue
+            self.pump_once()
 
-            data_messages = self._get_latest_mocap_packet()
-            if data_messages is None:
-                data_messages = self._get_last_mocap_frame()
-            if data_messages is None:
-                continue
+    def pump_once(self):
+        """Send the latest (or last) mocap frame to every connected client, once.
 
-            for client in clients:
-                try:
-                    self._send_data_packet(client, data_messages)
-                except ValueError as e:
-                    print(str(e))
-                    continue
+        Safe to call from any thread; the Isaac wrapper calls this from its
+        physics-step callback so frame delivery does not depend on the background
+        data thread getting scheduled inside the GIL-bound Isaac Sim process.
+        """
+        with self.clients_lock:
+            clients = list(self.connected_clients)
+        if not clients:
+            return
+
+        data_messages = self._get_latest_mocap_packet()
+        if data_messages is None:
+            data_messages = self._get_last_mocap_frame()
+        if data_messages is None:
+            return
+
+        for client in clients:
+            try:
+                self._send_data_packet(client, data_messages)
+            except ValueError as e:
+                print(str(e))
+                continue
 
     def _command_listener_loop(self):
         # Listens on UDP command socket for incoming command requests from clients.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
new file mode 100644
index 000000000..8bbab1809
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
@@ -0,0 +1,78 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Pure pose->frame builder tests (no USD, no Kit)."""
+
+from __future__ import annotations
+
+import math
+import struct
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.frames import (
+    MODEL_LIST_CHANGED,
+    TRACKING_VALID,
+    BodySample,
+    build_frame,
+    make_rigid_body_data,
+)
+
+pytestmark = pytest.mark.unit
+
+
+def test_make_rigid_body_data_copies_pose_and_sets_valid_bit():
+    rb = make_rigid_body_data(
+        BodySample(7, (1.0, 2.0, 3.0), (0.0, 0.0, 0.7071068, 0.7071068), valid=True)
+    )
+    assert rb.ID == 7
+    assert (rb.x, rb.y, rb.z) == (1.0, 2.0, 3.0)
+    assert rb.qw == pytest.approx(0.7071068)
+    assert rb.params & TRACKING_VALID  # client requires this bit or it skips the body
+
+
+def test_lost_sample_clears_valid_bit_and_is_nan():
+    rb = make_rigid_body_data(BodySample.lost(3))
+    assert rb.ID == 3
+    assert rb.params & TRACKING_VALID == 0
+    assert math.isnan(rb.x) and math.isnan(rb.y) and math.isnan(rb.z)
+
+
+def test_build_frame_no_bodies():
+    frame = build_frame(0, [])
+    assert frame.iFrame == 0
+    assert frame.nRigidBodies == 0
+    assert frame.params == 0
+
+
+def test_build_frame_multiple_bodies_preserve_order():
+    samples = [
+        BodySample(1, (1.0, 0.0, 0.0)),
+        BodySample(2, (0.0, 2.0, 0.0)),
+        BodySample(5, (0.0, 0.0, 3.0)),
+    ]
+    frame = build_frame(42, samples)
+    assert frame.iFrame == 42
+    assert frame.nRigidBodies == 3
+    assert frame.RigidBodies[0].ID == 1 and frame.RigidBodies[0].x == 1.0
+    assert frame.RigidBodies[1].ID == 2 and frame.RigidBodies[1].y == 2.0
+    assert frame.RigidBodies[2].ID == 5 and frame.RigidBodies[2].z == 3.0
+
+
+def test_model_list_changed_sets_frame_param_bit():
+    assert build_frame(0, [], model_list_changed=True).params & MODEL_LIST_CHANGED
+    assert build_frame(0, [], model_list_changed=False).params & MODEL_LIST_CHANGED == 0
+
+
+def test_frame_packs_and_rigid_body_section_decodes():
+    frame = build_frame(9, [BodySample(4, (1.5, -2.5, 3.5), (0.0, 0.0, 0.0, 1.0))])
+    payload = frame.pack(natnet_major=4, natnet_minor=4)
+
+    # iFrame, then 4.4 counted sections (count+size each) for markersets & other markers.
+    (iframe,) = struct.unpack_from("<i", payload, 0)
+    assert iframe == 9
+    # rigidbodies section: offset 4 + (4+4) markersets + (4+4) othermarkers = 20
+    rb_count, rb_size = struct.unpack_from("<ii", payload, 20)
+    assert rb_count == 1
+    body_id, x, y, z = struct.unpack_from("<i3f", payload, 28)
+    assert body_id == 4
+    assert (round(x, 3), round(y, 3), round(z, 3)) == (1.5, -2.5, 3.5)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
new file mode 100644
index 000000000..ac31c22b4
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
@@ -0,0 +1,202 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Pose sampling + dynamic catalog (dirty/resync) tests against an in-memory stage.
+
+Guarded by ``importorskip("pxr")``. Uses a fake server (records enqueued frames and
+MODELDEF payloads) so no sockets are bound — we exercise the manager's sampling and
+resync logic, not the wire.
+"""
+
+from __future__ import annotations
+
+import math
+
+import pytest
+
+pytest.importorskip("pxr")
+
+from pxr import Gf, Usd, UsdGeom  # noqa: E402
+
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig  # noqa: E402
+from optitrack.natnet.emulator.isaac.frames import MODEL_LIST_CHANGED, TRACKING_VALID  # noqa: E402
+from optitrack.natnet.emulator.isaac.manager import NatNetServerManager  # noqa: E402
+from optitrack.natnet.emulator.isaac.usd_bindings import author_interface, read_world_pose  # noqa: E402
+
+pytestmark = pytest.mark.unit
+
+
+class FakeServer:
+    def __init__(self):
+        self.frames = []
+        self.payloads = []
+
+    def set_model_def_payload(self, payload):
+        self.payloads.append(payload)
+
+    def start(self):
+        pass
+
+    def shutdown(self):
+        pass
+
+    def enqueue_mocap_data(self, frame):
+        self.frames.append(frame)
+
+
+def _xform(stage, path, translate=(0.0, 0.0, 0.0)):
+    xform = UsdGeom.Xform.Define(stage, path)
+    xform.AddTranslateOp().Set(Gf.Vec3d(*translate))
+    return xform
+
+
+def _manager_with_fake():
+    fake = FakeServer()
+    mgr = NatNetServerManager(server_factory=lambda cfg: fake)
+    return mgr, fake
+
+
+# --- read_world_pose -------------------------------------------------------------
+
+
+def test_read_world_pose_returns_translation():
+    stage = Usd.Stage.CreateInMemory()
+    _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0))
+    pose = read_world_pose(stage.GetPrimAtPath("/World/base_link"))
+    assert pose is not None
+    (x, y, z), (qx, qy, qz, qw) = pose
+    assert (round(x, 3), round(y, 3), round(z, 3)) == (1.0, 2.0, 3.0)
+    assert qw == pytest.approx(1.0)
+
+
+def test_read_world_pose_invalid_prim_is_none():
+    stage = Usd.Stage.CreateInMemory()
+    assert read_world_pose(stage.GetPrimAtPath("/World/nope")) is None
+
+
+# --- sample_once -----------------------------------------------------------------
+
+
+def test_sample_once_no_bodies():
+    stage = Usd.Stage.CreateInMemory()
+    author_interface(stage, "/World/NatNetInterface", NatNetInterfaceConfig())
+    mgr, fake = _manager_with_fake()
+    mgr.start_server(NatNetInterfaceConfig(server_ip="127.0.0.1"))
+    frame = mgr.sample_once(stage)
+    assert frame is not None and frame.nRigidBodies == 0
+
+
+def test_sample_once_streams_world_pose():
+    stage = Usd.Stage.CreateInMemory()
+    _xform(stage, "/World/base_link", translate=(4.0, 5.0, 6.0))
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/base_link", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    frame = mgr.sample_once(stage)
+    assert frame.nRigidBodies == 1
+    rb = frame.RigidBodies[0]
+    assert rb.ID == 1
+    assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (4.0, 5.0, 6.0)
+    assert rb.params & TRACKING_VALID
+    # First frame after start resyncs -> client should be told the model list changed.
+    assert frame.params & MODEL_LIST_CHANGED
+
+
+def test_sample_once_missing_prim_is_lost():
+    stage = Usd.Stage.CreateInMemory()
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", bodies=[BodyBinding("Ghost", "/World/missing", 9)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    frame = mgr.sample_once(stage)
+    rb = frame.RigidBodies[0]
+    assert rb.ID == 9
+    assert rb.params & TRACKING_VALID == 0
+    assert math.isnan(rb.x)
+
+
+def test_moving_prim_updates_streamed_position():
+    stage = Usd.Stage.CreateInMemory()
+    xform = _xform(stage, "/World/base_link", translate=(0.0, 0.0, 0.0))
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/base_link", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    mgr.sample_once(stage)
+    xform.GetOrderedXformOps()[0].Set(Gf.Vec3d(10.0, 0.0, 0.0))
+    frame = mgr.sample_once(stage)
+    assert round(frame.RigidBodies[0].x, 3) == 10.0
+
+
+def test_body_added_while_live_is_picked_up_on_resync():
+    stage = Usd.Stage.CreateInMemory()
+    _xform(stage, "/World/a", translate=(1.0, 0.0, 0.0))
+    _xform(stage, "/World/b", translate=(0.0, 2.0, 0.0))
+    cfg1 = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", bodies=[BodyBinding("A", "/World/a", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg1)
+    mgr, fake = _manager_with_fake()
+    mgr.start_server(cfg1)
+
+    first = mgr.sample_once(stage)
+    assert first.nRigidBodies == 1
+
+    # Add a second body live: re-author the prim, then mark dirty (the UI/USD-notice
+    # path calls mark_dirty for us in Kit).
+    cfg2 = NatNetInterfaceConfig(
+        server_ip="127.0.0.1",
+        bodies=[BodyBinding("A", "/World/a", 1), BodyBinding("B", "/World/b", 2)],
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg2)
+    mgr.mark_dirty()
+
+    second = mgr.sample_once(stage)
+    assert second.nRigidBodies == 2
+    assert second.params & MODEL_LIST_CHANGED  # catalog grew -> tell the client
+    ids = {second.RigidBodies[i].ID for i in range(second.nRigidBodies)}
+    assert ids == {1, 2}
+    # MODELDEF payload was refreshed on the server for the new catalog.
+    assert len(fake.payloads) >= 2
+
+
+def test_target_prim_created_after_start_becomes_valid():
+    """A body whose target prim is spawned *after* the server starts (e.g. a Pegasus
+    drone base_link created on the first Play tick) must start streaming a valid pose
+    as soon as the prim appears — no mark_dirty/resync required, because the target
+    path is re-resolved every sample."""
+    stage = Usd.Stage.CreateInMemory()
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/drone1/base_link", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, _fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    # Prim does not exist yet -> lost.
+    first = mgr.sample_once(stage)
+    assert first.RigidBodies[0].params & TRACKING_VALID == 0
+
+    # Spawn the target prim later (simulating the Play-tick drone creation).
+    _xform(stage, "/World/drone1/base_link", translate=(7.0, 8.0, 9.0))
+
+    # Next sample re-resolves the path -> valid pose, with no mark_dirty().
+    second = mgr.sample_once(stage)
+    rb = second.RigidBodies[0]
+    assert rb.params & TRACKING_VALID
+    assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (7.0, 8.0, 9.0)
+
+
+def test_sample_once_noop_without_server():
+    stage = Usd.Stage.CreateInMemory()
+    mgr, _fake = _manager_with_fake()
+    assert mgr.sample_once(stage) is None
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
new file mode 100644
index 000000000..84e2f9896
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
@@ -0,0 +1,87 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""End-to-end: manager samples a USD prim and streams its pose over the wire.
+
+Real server + real UDP sockets (loopback) + an in-memory USD stage. Proves the full
+data-enqueue path: ``sample_once`` reads the prim's world pose, builds a frame, the
+server relays it as ``NAT_FRAMEOFDATA``, and the bytes carry the sampled position.
+Guarded by ``importorskip("pxr")``.
+"""
+
+from __future__ import annotations
+
+import socket
+import struct
+import time
+
+import pytest
+
+pytest.importorskip("pxr")
+
+from pxr import Gf, Usd, UsdGeom  # noqa: E402
+
+from natnet_test_helpers import NatNetTestClient, ephemeral_udp_port  # noqa: E402
+
+from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig  # noqa: E402
+from optitrack.natnet.emulator.isaac.manager import NatNetServerManager  # noqa: E402
+from optitrack.natnet.emulator.isaac.usd_bindings import author_interface  # noqa: E402
+from optitrack.natnet.emulator.server import natnet_server_types as st  # noqa: E402
+
+pytestmark = pytest.mark.unit
+
+
+def _decode_first_rigid_body(payload: bytes):
+    # iFrame(4) + markersets(count4+size4) + othermarkers(count4+size4) = 20, then
+    # rigid bodies: count(4) + size(4) at 20, first body at 28: id + xyz.
+    rb_count, _rb_size = struct.unpack_from("<ii", payload, 20)
+    body_id, x, y, z = struct.unpack_from("<i3f", payload, 28)
+    return rb_count, body_id, (x, y, z)
+
+
+def test_streamed_frame_carries_sampled_usd_pose():
+    stage = Usd.Stage.CreateInMemory()
+    xform = UsdGeom.Xform.Define(stage, "/World/base_link")
+    xform.AddTranslateOp().Set(Gf.Vec3d(7.0, -3.0, 2.0))
+
+    command_port = ephemeral_udp_port()
+    data_port = ephemeral_udp_port()
+    while data_port == command_port:
+        data_port = ephemeral_udp_port()
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1",
+        command_port=command_port,
+        data_port=data_port,
+        bodies=[BodyBinding("Drone", "/World/base_link", 1)],
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+
+    mgr = NatNetServerManager()  # real factory
+    assert mgr.start_from_stage.__self__ is mgr  # scripting entry exists
+    assert mgr.start_server(cfg) is True
+
+    client = NatNetTestClient(timeout=2.0)
+    try:
+        client.send_message(command_port, st.MessageId.NAT_CONNECT)
+        client.recv_message()
+
+        got = None
+        deadline = time.time() + 2.0
+        while time.time() < deadline:
+            mgr.sample_once(stage)  # stand in for the physics step callback
+            try:
+                message_id, payload, addr = client.recv_message()
+            except socket.timeout:
+                continue
+            if message_id == int(st.MessageId.NAT_FRAMEOFDATA):
+                assert addr[1] == data_port
+                got = payload
+                break
+    finally:
+        client.close()
+        mgr.stop_server()
+
+    assert got is not None, "expected a NAT_FRAMEOFDATA frame"
+    rb_count, body_id, (x, y, z) = _decode_first_rigid_body(got)
+    assert rb_count == 1
+    assert body_id == 1
+    assert (round(x, 3), round(y, 3), round(z, 3)) == (7.0, -3.0, 2.0)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
new file mode 100644
index 000000000..d58bd9a1a
--- /dev/null
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
@@ -0,0 +1,87 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Hermetic tests for the launch-script helper (no USD / Kit).
+
+``build_drone_config`` is the pure half of ``scene_setup`` used by the Pegasus
+example launch scripts to author one rigid body per drone base_link on load.
+"""
+
+from __future__ import annotations
+
+import pytest
+
+from optitrack.natnet.emulator.isaac.config import NatNetInterfaceConfig
+from optitrack.natnet.emulator.isaac.scene_setup import (
+    DEFAULT_INTERFACE_PATH,
+    build_drone_config,
+)
+
+pytestmark = pytest.mark.unit
+
+
+def test_default_interface_path():
+    assert DEFAULT_INTERFACE_PATH == "/World/NatNetInterface"
+
+
+def test_single_drone_maps_name_id_and_target():
+    cfg = build_drone_config([("Drone", 1, "/World/base_link")])
+    assert isinstance(cfg, NatNetInterfaceConfig)
+    assert cfg.server_enabled is True
+    assert cfg.server_ip == "172.31.0.200"  # Isaac container default
+    assert cfg.mode == "unicast"
+    assert len(cfg.bodies) == 1
+    body = cfg.bodies[0]
+    assert body.rigid_body_name == "Drone"
+    assert body.streaming_id == 1
+    assert body.target_prim == "/World/base_link"
+
+
+def test_multi_drone_unique_names_and_ids():
+    drones = [(f"Drone{i}", i, f"/World/drone{i}/base_link") for i in range(1, 4)]
+    cfg = build_drone_config(drones)
+    assert len(cfg.bodies) == 3
+    assert [b.streaming_id for b in cfg.bodies] == [1, 2, 3]
+    assert [b.rigid_body_name for b in cfg.bodies] == ["Drone1", "Drone2", "Drone3"]
+    assert [b.target_prim for b in cfg.bodies] == [
+        "/World/drone1/base_link",
+        "/World/drone2/base_link",
+        "/World/drone3/base_link",
+    ]
+    # Must pass the config validator (unique names/ids, valid ports).
+    cfg.validate()
+
+
+def test_no_drones_yields_empty_catalog():
+    cfg = build_drone_config([])
+    assert cfg.bodies == []
+    cfg.validate()
+
+
+def test_server_params_are_forwarded():
+    cfg = build_drone_config(
+        [("Drone", 1, "/World/base_link")],
+        server_ip="10.0.0.5",
+        command_port=2510,
+        data_port=2511,
+        publish_rate=120.0,
+        server_enabled=False,
+    )
+    assert cfg.server_ip == "10.0.0.5"
+    assert cfg.command_port == 2510
+    assert cfg.data_port == 2511
+    assert cfg.publish_rate == 120.0
+    assert cfg.server_enabled is False
+
+
+def test_duplicate_names_raise():
+    with pytest.raises(ValueError):
+        build_drone_config(
+            [("Drone", 1, "/World/a"), ("Drone", 2, "/World/b")]
+        )
+
+
+def test_duplicate_ids_raise():
+    with pytest.raises(ValueError):
+        build_drone_config(
+            [("DroneA", 1, "/World/a"), ("DroneB", 1, "/World/b")]
+        )
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
index 0c9c7b082..f6d0f2de1 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
@@ -39,6 +39,10 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
+# Make the OptiTrack NatNet emulator package importable without enabling the Kit
+# UI extension (keeps a single, script-owned server manager).
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator")))
+
 
 # --------------------- CONFIGURATION ---------------------
 ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
@@ -48,6 +52,14 @@
 
 NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1"))
 ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true"
+
+# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface
+# prim with every drone's base_link as a tracked rigid body and start the server on
+# load. Mirrors the robot-side LAUNCH_NATNET gate. Each drone i is streamed as
+# "<NATNET_BODY_NAME><i>" (streaming id i); with the default natnet_config.yaml
+# (body_id=-1) each robot publishes all bodies, so narrow per robot with body_id.
+LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on")
+NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
 # ---------------------------------------------------------
 
 
@@ -172,8 +184,37 @@ def __init__(self):
         for i in range(1, NUM_ROBOTS + 1):
             spawn_drone(i)
 
+        # ----- OptiTrack / NatNet emulator -----
+        # Author a NatNet interface prim with every drone's base_link as a tracked
+        # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET).
+        self.natnet_manager = None
+        if LAUNCH_NATNET:
+            self._setup_natnet(stage)
+
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
 
+    def _setup_natnet(self, stage):
+        """Author the NatNet interface prim (one body per drone) and start the server."""
+        try:
+            from optitrack.natnet.emulator.isaac import start_drone_natnet_server
+
+            # Single agent: stream the bare body name ("Drone") so it matches the
+            # robot natnet_config body_name, the MAVROS vision-pose bridge, and the
+            # liveliness sentinel. Multi-drone uses unique indexed names (future:
+            # narrow each robot to its own body via body_id).
+            def _body_name(i: int) -> str:
+                return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{i}"
+
+            drones = [
+                (_body_name(i), i, f"/World/drone{i}/base_link")
+                for i in range(1, NUM_ROBOTS + 1)
+            ]
+            self.natnet_manager = start_drone_natnet_server(stage, drones)
+            carb.log_warn(f"[natnet] Emulator started with {len(drones)} body(ies).")
+        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
+            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
+            self.natnet_manager = None
+
     def run(self):
         if self.play_on_start:
             self.timeline.play()
@@ -192,6 +233,8 @@ def run(self):
                 app.update()
 
         carb.log_warn("Closing simulation.")
+        if self.natnet_manager is not None:
+            self.natnet_manager.on_shutdown()
         self.timeline.stop()
         simulation_app.close()
 
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
index 11819fc2f..d173b551f 100755
--- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
@@ -97,11 +97,22 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
+# Make the OptiTrack NatNet emulator package importable without enabling the Kit
+# UI extension (keeps a single, script-owned server manager).
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator")))
+
 
 # --------------------- CONFIGURATION ---------------------
 # Environment to load. Swap this URL/key for any other scene.
 ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
 
+# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface
+# prim with the drone's base_link as a tracked rigid body and start the server on
+# load. Mirrors the robot-side LAUNCH_NATNET gate (natnet_ros2 client). The body
+# name defaults to "Drone" to match natnet_config.yaml / the liveliness sentinel.
+LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on")
+NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
+
 # Scale applied to /World/stage. 0.01 converts cm→m for Nucleus assets.
 # Set to 1.0 if the environment is already in meters.
 STAGE_SCALE = 1.0
@@ -244,8 +255,29 @@ def __init__(self):
             min_range=0.75,
         )
 
+        # ----- OptiTrack / NatNet emulator -----
+        # Author a NatNet interface prim with the drone's base_link as a tracked
+        # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET).
+        self.natnet_manager = None
+        if LAUNCH_NATNET:
+            self._setup_natnet(stage)
+
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
 
+    def _setup_natnet(self, stage):
+        """Author the NatNet interface prim and start the emulator server."""
+        try:
+            from optitrack.natnet.emulator.isaac import start_drone_natnet_server
+
+            drones = [(NATNET_BODY_NAME, 1, "/World/base_link")]
+            self.natnet_manager = start_drone_natnet_server(stage, drones)
+            carb.log_warn(
+                f"[natnet] Emulator started with 1 body ('{NATNET_BODY_NAME}' -> /World/base_link)."
+            )
+        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
+            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
+            self.natnet_manager = None
+
     def run(self):
 
         if self.play_on_start:
@@ -267,6 +299,8 @@ def run(self):
                 app.update()
 
         carb.log_warn("Closing simulation.")
+        if self.natnet_manager is not None:
+            self.natnet_manager.on_shutdown()
         self.timeline.stop()
         simulation_app.close()
 
diff --git a/tests/docker/docker-compose.yaml b/tests/docker/docker-compose.yaml
index 4e392d284..28bde3275 100644
--- a/tests/docker/docker-compose.yaml
+++ b/tests/docker/docker-compose.yaml
@@ -13,5 +13,10 @@ services:
     environment:
       - AIRSTACK_ROOT=${AIRSTACK_PATH}
       - DISPLAY=${DISPLAY:-}
+      # Forward the OptiTrack/NatNet toggle so the in-runner `airstack up`
+      # (which does os.environ.copy()) brings the stack up with NatNet enabled and
+      # the test_natnet_pose_alive sentinel runs instead of skipping.
+      - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
+      - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone}
     working_dir: ${AIRSTACK_PATH}/tests
     network_mode: host
diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md
index 8a004282a..7e8919aa3 100644
--- a/tests/integration/natnet/README.md
+++ b/tests/integration/natnet/README.md
@@ -8,9 +8,17 @@ Marks: `integration` (tier) + `natnet` (scenario).
 
 ## What it verifies
 
-1. Emulator serves the default Drone MODELDEF and streams frames with rigid-body `ID=1`.
-2. `natnet_ros2_node` connects via unicast, parses MODELDEF, and publishes
-   `/{ROBOT_NAME}/perception/optitrack/Drone` at ≥ 5 Hz.
+Two variants, both ending at `/{ROBOT_NAME}/perception/optitrack/Drone` ≥ 5 Hz:
+
+1. **Raw server** (`test_natnet_ros2_receives_drone_pose_hz`) — hand-built frames
+   via `NatNetUnicastServer` (no USD): the minimal end-to-end wire check.
+2. **Isaac wrapper** (`test_natnet_ros2_receives_isaac_wrapper_pose_hz`) — the full
+   new data path: `NatNetServerManager` builds the catalog from a
+   `NatNetInterfaceConfig` and samples a moving prim's world pose off an in-memory
+   USD stage (`sample_once`, exactly what the in-sim physics-step callback does),
+   streaming real frames to the robot client. Skips without `usd-core` (`pxr`).
+   Exact pose-value fidelity is covered hermetically by the package's
+   `test_pose_streaming.py` loopback.
 
 ## Requirements
 
@@ -54,14 +62,21 @@ which calls `set_model_def_payload()`. See
 [`defaults.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py)
 for hardcoded Drone reference constants used in tests.
 
-## Future: Isaac-wrapped variant + liveliness
-
-Today the NatNet **server** is the host emulator. Once the Isaac-sim emulator
-wrapper emits NatNet frames from the simulator, an Isaac-wrapped variant will be
-added in this directory, and the gated pose-rate check can additionally surface
-as a conditional sentinel in
-[`../../system/test_liveliness.py`](../../system/test_liveliness.py) (run only
-when `LAUNCH_NATNET=true`).
+## Liveliness sentinel (sim end-to-end)
+
+The integration tier drives the emulator **host-side** (no sim/GPU). The matching
+in-sim check is a conditional sentinel in
+[`../../system/test_liveliness.py`](../../system/test_liveliness.py):
+`TestLiveliness::test_natnet_pose_alive` asserts
+`/{robot}/perception/optitrack/<body>` is live per robot. It is **gated on
+`LAUNCH_NATNET=true`** (skips otherwise), so normal liveliness runs are unaffected.
+Override the body name with `NATNET_BODY_NAME` (default `Drone`).
+
+> The sentinel passes once the Isaac emulator actually streams in-sim — i.e. a
+> Pegasus launch script authors a `NatNetInterface` prim and calls
+> `NatNetServerManager.start_from_stage()` (the scripting entry point). Until that
+> sim auto-start is wired, run the **integration** variants above for robot-level
+> coverage.
 
 ## libNatNet 4.4 unicast — verified wire contract
 
diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py
index b95376797..6a98551c1 100644
--- a/tests/integration/natnet/test_natnet_integration.py
+++ b/tests/integration/natnet/test_natnet_integration.py
@@ -104,8 +104,67 @@ def _frame_publisher(server: NatNetUnicastServer, stop_event: threading.Event) -
         time.sleep(interval)
 
 
+def _launch_natnet_node(container, host_ip, command_port, domain_id):
+    """Start natnet_ros2_node in the container pointed at the host emulator."""
+    launch_cmd = (
+        f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && "
+        f"exec {_NATNET_NODE} --ros-args "
+        f"-p server_ip:={host_ip} "
+        f"-p command_port:={command_port} "
+        f"-p body_name:=Drone "
+        f"-p body_id:=1 "
+        f"-p publish_to_mavros:=false "
+        f"-p publish_direct_optitrack:=true'"
+    )
+    return subprocess.Popen(
+        ["docker", "exec", container, "bash", "-c", launch_cmd],
+        stdout=subprocess.PIPE,
+        stderr=subprocess.STDOUT,
+        text=True,
+    )
+
+
+def _assert_pose_stream(container, robot_name, domain_id):
+    """Wait for the pose topic then assert a sustained rate >= _MIN_HZ."""
+    pose_topic = f"/{robot_name}/perception/optitrack/Drone"
+    pose_cov_topic = f"{pose_topic}/pose_cov"
+
+    time.sleep(_WARMUP_S)
+    first_msg_s = wait_for_first_message(
+        container, pose_cov_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S)
+    )
+    assert first_msg_s is not None, (
+        f"No messages on {pose_cov_topic} within {_STREAM_HOLD_S}s "
+        "(NatNet connect or frame stream failed)"
+    )
+    hz = sample_hz(
+        container,
+        pose_topic,
+        domain_id,
+        _ROBOT_SETUP,
+        duration=min(8, int(_STREAM_HOLD_S - first_msg_s)),
+        window=20,
+    )
+    assert hz is not None, f"No sustained stream on {pose_topic}"
+    assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}"
+
+
+def _terminate(proc) -> None:
+    if proc is None:
+        return
+    proc.terminate()
+    try:
+        proc.wait(timeout=5)
+    except subprocess.TimeoutExpired:
+        proc.kill()
+
+
 def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
-    """Emulator on host streams dummy Drone frames while natnet_ros2_node publishes."""
+    """Emulator on host streams dummy Drone frames while natnet_ros2_node publishes.
+
+    Raw-server variant: hand-built frames via ``NatNetUnicastServer`` (no Isaac
+    wrapper, no USD) — the minimal end-to-end wire check.
+    """
     container = robot_autonomy_stack["container"]
 
     if not _natnet_node_available(container):
@@ -120,8 +179,6 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
     command_port = ephemeral_udp_port(host_ip)
     robot_name = _container_env(container, "ROBOT_NAME", "robot_1")
     domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0"))
-    pose_topic = f"/{robot_name}/perception/optitrack/Drone"
-    pose_cov_topic = f"{pose_topic}/pose_cov"
 
     server = NatNetUnicastServer(
         local_interface=host_ip,
@@ -133,66 +190,98 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
 
     stop_event = threading.Event()
     publisher = threading.Thread(
-        target=_frame_publisher,
-        args=(server, stop_event),
-        daemon=True,
+        target=_frame_publisher, args=(server, stop_event), daemon=True
     )
 
     node_proc: subprocess.Popen[str] | None = None
     try:
-        # Seed dummy frames before the client connects; keep streaming for the full hold window.
+        # Seed dummy frames before the client connects; keep streaming the whole window.
         publisher.start()
         time.sleep(0.1)
         server.start()
+        node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id)
+        _assert_pose_stream(container, robot_name, domain_id)
+    finally:
+        stop_event.set()
+        publisher.join(timeout=2.0)
+        _terminate(node_proc)
+        server.shutdown()
 
-        launch_cmd = (
-            f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && "
-            f"exec {_NATNET_NODE} --ros-args "
-            f"-p server_ip:={host_ip} "
-            f"-p command_port:={command_port} "
-            f"-p body_name:=Drone "
-            f"-p body_id:=1 "
-            f"-p publish_to_mavros:=false "
-            f"-p publish_direct_optitrack:=true'"
-        )
-        node_proc = subprocess.Popen(
-            ["docker", "exec", container, "bash", "-c", launch_cmd],
-            stdout=subprocess.PIPE,
-            stderr=subprocess.STDOUT,
-            text=True,
-        )
 
-        time.sleep(_WARMUP_S)
-        first_msg_s = wait_for_first_message(
-            container,
-            pose_cov_topic,
-            domain_id,
-            _ROBOT_SETUP,
-            timeout=int(_STREAM_HOLD_S),
-        )
-        assert first_msg_s is not None, (
-            f"No messages on {pose_cov_topic} within {_STREAM_HOLD_S}s "
-            "(NatNet connect or frame stream failed)"
-        )
+def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack):
+    """Isaac-wrapper variant: the full new data path drives natnet_ros2.
 
-        # Server still streaming — measure sustained rate over the remaining hold window.
-        hz = sample_hz(
-            container,
-            pose_topic,
-            domain_id,
-            _ROBOT_SETUP,
-            duration=min(8, int(_STREAM_HOLD_S - first_msg_s)),
-            window=20,
-        )
-        assert hz is not None, f"No sustained stream on {pose_topic}"
-        assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}"
+    ``NatNetServerManager`` builds the catalog from a ``NatNetInterfaceConfig``,
+    samples a (moving) prim's world pose off an in-memory USD stage via
+    ``sample_once`` — exactly what the physics-step callback does in-sim — and
+    streams real frames. We assert ``natnet_ros2`` connects and publishes the pose
+    at a stable rate. (Exact pose-value fidelity is covered hermetically by the
+    package's ``test_pose_streaming.py`` loopback; here we prove the wrapper feeds
+    the *real* robot client end to end without a sim/GPU.)
+    """
+    pytest.importorskip("pxr")
+    import math
+
+    from pxr import Gf, Usd, UsdGeom
+
+    from optitrack.natnet.emulator.isaac import (
+        BodyBinding,
+        NatNetInterfaceConfig,
+        NatNetServerManager,
+        author_interface,
+    )
+
+    container = robot_autonomy_stack["container"]
+    if not _natnet_node_available(container):
+        pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)")
+
+    _stop_stale_natnet_nodes(container)
+
+    host_ip = _docker_default_gateway(container)
+    command_port = ephemeral_udp_port(host_ip)
+    data_port = ephemeral_udp_port(host_ip)
+    while data_port == command_port:
+        data_port = ephemeral_udp_port(host_ip)
+    robot_name = _container_env(container, "ROBOT_NAME", "robot_1")
+    domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0"))
+
+    stage = Usd.Stage.CreateInMemory()
+    xform = UsdGeom.Xform.Define(stage, "/World/base_link")
+    translate_op = xform.AddTranslateOp()
+    translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0))
+    cfg = NatNetInterfaceConfig(
+        server_ip=host_ip,
+        command_port=command_port,
+        data_port=data_port,
+        publish_rate=50.0,
+        bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)],
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+
+    manager = NatNetServerManager(server_factory=None)  # real server factory
+    stop_event = threading.Event()
+
+    def _sampler():
+        # Stand in for the in-sim physics-step callback: move the prim and sample.
+        interval = 1.0 / cfg.publish_rate
+        t = 0.0
+        while not stop_event.is_set():
+            translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0))
+            manager.sample_once(stage)
+            t += interval
+            time.sleep(interval)
+
+    sampler = threading.Thread(target=_sampler, daemon=True)
+
+    node_proc: subprocess.Popen[str] | None = None
+    try:
+        assert manager.start_server(cfg) is True
+        sampler.start()
+        time.sleep(0.1)
+        node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id)
+        _assert_pose_stream(container, robot_name, domain_id)
     finally:
         stop_event.set()
-        publisher.join(timeout=2.0)
-        if node_proc is not None:
-            node_proc.terminate()
-            try:
-                node_proc.wait(timeout=5)
-            except subprocess.TimeoutExpired:
-                node_proc.kill()
-        server.shutdown()
+        sampler.join(timeout=2.0)
+        _terminate(node_proc)
+        manager.stop_server()
diff --git a/tests/sim/optitrack_natnet_emulator/test_frames.py b/tests/sim/optitrack_natnet_emulator/test_frames.py
new file mode 100644
index 000000000..493abb722
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_frames.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator pose->frame builder unit tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_frames.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
new file mode 100644
index 000000000..0ad8dc5e6
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator pose-sampling / resync unit tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_pose_sampling.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
new file mode 100644
index 000000000..f33b6d251
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator end-to-end pose-streaming test."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_pose_streaming.py",
+)
diff --git a/tests/sim/optitrack_natnet_emulator/test_scene_setup.py b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py
new file mode 100644
index 000000000..4666c644a
--- /dev/null
+++ b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2024 Carnegie Mellon University
+# MIT License - see LICENSE in the repository root for full text.
+"""Proxy: re-exposes optitrack.natnet.emulator launch-helper (scene_setup) tests."""
+
+from conftest import reexport_unit_tests, repo_path
+
+reexport_unit_tests(
+    globals(),
+    repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
+    "test_scene_setup.py",
+)
diff --git a/tests/system/test_liveliness.py b/tests/system/test_liveliness.py
index 342e5f49a..b465726d5 100644
--- a/tests/system/test_liveliness.py
+++ b/tests/system/test_liveliness.py
@@ -7,6 +7,7 @@
 Sensor topic rates, bridge stereo Hz, LiDAR echo/sanity, and sim RTF live in
 ``system/test_sensors.py`` (``@pytest.mark.sensors``), ordered after this module.
 """
+import os
 import time
 
 import pytest
@@ -21,6 +22,7 @@
     logger,
     ros2_exec,
     sample_compute_usage,
+    sample_hz,
     wait_for_first_message,
 )
 
@@ -31,6 +33,19 @@
     "/robot_{N}/trajectory_controller/trajectory_control_node",
 ]
 
+# Optional OptiTrack/NatNet liveliness sentinel — only meaningful when the robot
+# launches natnet_ros2 (LAUNCH_NATNET=true) and the sim streams NatNet frames
+# (Isaac emulator wrapper). Off by default, so normal runs skip it cleanly.
+_NATNET_ENABLED = os.environ.get("LAUNCH_NATNET", "").strip().lower() in ("1", "true", "yes", "on")
+_NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
+_NATNET_MIN_HZ = 5.0
+# Generous first-message budget: on a cold Isaac boot the standalone Pegasus
+# script must finish loading, start Play (physics steps drive the emulator's
+# synchronous frame pump), and the robot natnet_ros2 client must connect over
+# UDP before pose frames appear. 60s was too tight vs the 600s /clock and 300s
+# sentinel-node budgets.
+_NATNET_FIRST_MSG_TIMEOUT = 120
+
 
 def _parse_panes(raw):
     """Return (crashed, active_count). Input lines: 'session:window|pane_pid|title|kids'.
@@ -251,6 +266,51 @@ def ready():
             fail_msg=lambda: f"sentinel nodes not ready after 300s: {last_msg[0]}",
         )
 
+    @pytest.mark.skipif(
+        not _NATNET_ENABLED,
+        reason="LAUNCH_NATNET not enabled — OptiTrack/NatNet pose sentinel skipped",
+    )
+    @pytest.mark.dependency(depends=["sim_ready", "nodes"])
+    def test_natnet_pose_alive(self, airstack_env):
+        """When NatNet is enabled, the OptiTrack pose topic must publish per robot.
+
+        Closes the loop the integration tier opens host-side: with the Isaac
+        emulator wrapper streaming in-sim and ``natnet_ros2`` running on each robot
+        (LAUNCH_NATNET=true), ``/{robot}/perception/optitrack/<body>/pose_cov`` must
+        be live. We probe ``/pose_cov`` (PoseWithCovarianceStamped) because it is
+        published unconditionally for every body, whereas the bare ``<body>``
+        PoseStamped is gated on the ``publish_direct_optitrack`` param.
+        """
+        cfg = airstack_env["cfg"]
+        robot_containers = get_robot_containers(airstack_env["robot_pattern"])
+        m = get_metrics()
+        tid = current_test_id()
+
+        failures = []
+        for n in range(1, airstack_env["num_robots"] + 1):
+            container = robot_containers[n - 1]
+            topic = f"/robot_{n}/perception/optitrack/{_NATNET_BODY_NAME}/pose_cov"
+            first = wait_for_first_message(
+                container,
+                topic,
+                domain_id=n,
+                setup_bash=cfg["robot_setup_bash"],
+                timeout=_NATNET_FIRST_MSG_TIMEOUT,
+            )
+            if first is None:
+                failures.append(
+                    f"robot_{n}: no message on {topic} within {_NATNET_FIRST_MSG_TIMEOUT}s"
+                )
+                continue
+            hz = sample_hz(
+                container, topic, domain_id=n, setup_bash=cfg["robot_setup_bash"], duration=5, window=20
+            )
+            m.record(tid, f"natnet_pose_hz_robot_{n}", hz if hz is not None else "none", unit="Hz")
+            if hz is None or hz < _NATNET_MIN_HZ:
+                failures.append(f"robot_{n}: {topic} at {hz} Hz (< {_NATNET_MIN_HZ})")
+
+        assert not failures, "NatNet pose sentinel failed: " + "; ".join(failures)
+
     @pytest.mark.dependency(depends=["sim_ready", "nodes", "tmux"])
     def test_stable(self, airstack_env, request):
         """Poll infra only: tmux, sentinel nodes, compute (no sensor topic Hz)."""

From bd37eda12ad14521e9c4cd3758b215b005603764 Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Thu, 11 Jun 2026 12:54:58 -0400
Subject: [PATCH 18/27] added capability to set z or y axis up (we should keep
 z-axis up, however)

---
 .../docs/natnet_interface_prim_design.md      | 160 +++++++++++++++++-
 .../optitrack/natnet/emulator/isaac/config.py |  14 ++
 .../optitrack/natnet/emulator/isaac/frames.py |  33 +++-
 .../natnet/emulator/isaac/manager.py          |  12 +-
 .../natnet/emulator/isaac/scene_setup.py      |   3 +
 .../natnet/emulator/isaac/ui_extension.py     |  13 +-
 .../natnet/emulator/isaac/usd_bindings.py     |   4 +
 .../test/test_frames.py                       |  23 +++
 .../test/test_interface_authoring.py          |  12 ++
 .../test/test_interface_config.py             |  15 ++
 .../test/test_pose_sampling.py                |  29 ++++
 11 files changed, 300 insertions(+), 18 deletions(-)

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
index a3770f3f8..8f05ef495 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
@@ -253,9 +253,18 @@ world transform (`UsdGeom.Xformable.ComputeLocalToWorldTransform` via a per-fram
 `server.enqueue_mocap_data(frame)`. No schema reads or stage searches happen in the
 hot path — only direct pose pulls off the cached handles, exactly as intended.
 
-**Frame convention caveat:** Isaac is typically Z-up / meters; Motive streams Y-up
-by convention. The sampler must emit the same convention `natnet_ros2` expects on
-the wire — to be pinned down during implementation (not a design blocker).
+**Frame convention (`up_axis` option):** Isaac/USD is Z-up; Motive exposes an
+"Up Axis" setting. The reference `natnet_ros2` driver (and ours) require it set to
+**Z** and do **no** axis conversion — `rb_to_pose` is an identity copy and
+`vision_pose_converter_node` only normalizes quaternion sign. So the interface
+config carries an `up_axis` (default **`"Z"`**) and the sampler streams the USD
+world pose **as-is** by default, matching the rest of the stack. Setting
+`up_axis="Y"` emulates a default Y-up Motive by rotating each streamed pose -90° about
+X (`(x,y,z) -> (x,z,-y)`, quaternion vector part the same, scalar kept) — a proper
+right-handed change of basis. The conversion is the pure, unit-tested
+`frames.to_motive_pose`; the manager re-reads `up_axis` on every resync and applies
+it in `sample_once`. Authored on the prim as `natnet:upAxis`, editable in the UI
+("Up axis" dropdown) and via `build_drone_config(..., up_axis=...)`.
 
 ## Packaging notes
 
@@ -334,7 +343,81 @@ Make the extension *aware* of interfaces without acting on them.
   path and fully parsed config; adding/removing a body or editing a field updates
   the logged readout live.
 
-### Commit 3 — Catalog parse + server start
+### Commit 5 — Data enqueue + dynamic catalog + scripting + UI readouts  ✅ IMPLEMENTED
+
+> Rolled the deferred data path together with live updates, scripting, and UI:
+> - **`isaac/frames.py`** (pure): `BodySample`, `make_rigid_body_data`, `build_frame`.
+>   Sets `params` bit `0x01` (tracking valid — the client *skips* bodies without it)
+>   and frame bit `0x02` (model list changed). **Frame convention resolved: emit the
+>   USD world pose as-is, no Z-up→Y-up swap** — `natnet_ros2`'s `rb_to_pose` is an
+>   identity copy and nothing downstream re-axes, so swapping would desync the pose.
+> - **`usd_bindings.read_world_pose(prim)`** — `ComputeLocalToWorldTransform` →
+>   `((x,y,z),(qx,qy,qz,qw))` (the pose *stored in the USD stage*).
+> - **Manager**: a **dirty flag** (`_needs_resync`) — set on any NatNet prim change
+>   (`_on_objects_changed`) or `mark_dirty()`, cleared by the next sample. The
+>   physics-step subscription (`subscribe_physics_step_events`) calls `sample_once`,
+>   which resyncs the catalog/target cache when dirty (so **bodies added/removed live
+>   are picked up**, refreshing the server MODELDEF and flagging `model_list_changed`),
+>   then samples every body's world pose and `enqueue_mocap_data`s one frame. Missing
+>   prims stream as lost (NaN, valid bit clear).
+> - **Scripting**: `NatNetServerManager.start_from_stage()` (author the prim, then
+>   call it). `start_server` / `apply_enabled` remain the programmatic enable path.
+> - **UI**: each tracked-body row has a live readout — status dot (green=streaming,
+>   grey=idle, red=missing) + world position pulled from USD, refreshed ~6 Hz.
+> - **Tests**: `test_frames.py` (pure: valid bit, NaN-lost, order, model-change bit,
+>   pack decode), `test_pose_sampling.py` (pxr: read_world_pose, sample no/one/missing,
+>   moving prim, **body-added-live resync + model_list_changed**), `test_pose_streaming.py`
+>   (pxr loopback: streamed `NAT_FRAMEOFDATA` carries the sampled USD position). Proxies
+>   added under `tests/sim/optitrack_natnet_emulator/`.
+
+### Commit 6 — Example-script auto-start (load-up integration)  ✅ IMPLEMENTED
+
+> The Pegasus example launch scripts stand the emulator up on scene load — no UI.
+> - **`isaac/scene_setup.py`** — `build_drone_config(drones, ...)` (pure: one
+>   `BodyBinding` per `(rigid_body_name, streaming_id, target_prim)`, validated) and
+>   `start_drone_natnet_server(stage, drones, ...)` (authors the `/World/NatNetInterface`
+>   prim, then creates a physics-subscribed `NatNetServerManager` and starts it).
+>   Exported from `optitrack.natnet.emulator.isaac`.
+> - **Launch scripts** (`launch_scripts/example_one*.py`, `example_multi*.py`): after
+>   spawning drones, if `LAUNCH_NATNET=true` they author one rigid body per drone
+>   `base_link` and start the server. Single: `("Drone", 1, /World/base_link)`.
+>   Multi: `("<NATNET_BODY_NAME><i>", i, /World/drone<i>/base_link)` for each drone.
+>   Failures are caught (never kill the sim); the manager is torn down on close.
+> - **Import path**: scripts add `../extensions/optitrack.natnet.emulator` to
+>   `sys.path` so the package imports without enabling the Kit UI extension (keeps a
+>   single, script-owned manager — no duplicate physics/USD subscriptions).
+> - **Wiring**: `LAUNCH_NATNET` / `NATNET_BODY_NAME` are passed into the isaac-sim
+>   container via `simulation/isaac-sim/docker/docker-compose.yaml`, mirroring the
+>   robot-side `natnet_ros2` gate. Server binds the container IP `172.31.0.200`
+>   (the config default), which the robot `server_ip` already points at.
+> - **Tests**: `test_scene_setup.py` (pure: single/multi mapping, server-param
+>   forwarding, duplicate name/id rejection, empty catalog). Proxy under
+>   `tests/sim/optitrack_natnet_emulator/`.
+
+### Commit 3 — Catalog parse + server start  ✅ IMPLEMENTED
+
+> **Status (combined with Commit 4):** catalog parse + server start/stop lifecycle
+> landed together. What shipped:
+> - `isaac/catalog.py`: `build_catalog(config) → sDataDescriptions` (pure ctypes,
+>   no USD) + `find_duplicate_targets(config)`.
+> - `usd_bindings.resolve_targets(stage, config) → (existing, missing)` diagnostics.
+> - `NatNetServerManager` owns **one** server via an injectable
+>   `server_factory` (default `default_server_factory`, unicast only). Methods:
+>   `start_server` (idempotent — builds catalog, `set_model_def_payload`, `start`),
+>   `stop_server`, `toggle_server`, `apply_enabled` (reconcile to `serverEnabled`),
+>   `is_running`, `log_target_diagnostics`. `on_shutdown` stops the server.
+> - UI: **Create Interface** (prim), plus a live **Start/Stop Server** toggle button
+>   + RUNNING/stopped status. The button starts/stops directly from the on-stage
+>   prim, **regardless** of `serverEnabled` (per request). Auto-reconcile of
+>   `serverEnabled` on scan is intentionally *not* wired (would fight the button);
+>   `apply_enabled` is available + tested for future auto-driving.
+> - Tests: `test_catalog.py` (no/single/multiple bodies, field fidelity, name
+>   truncation, MAX_MODELS guard, duplicate targets), `test_server_lifecycle.py`
+>   (mock factory: create+start exactly once, idempotent, toggle, reconcile,
+>   on_shutdown, multicast rejected), `test_target_resolution.py` (pxr-guarded:
+>   missing/empty/duplicate/mixed), `test_server_from_config.py` (loopback: served
+>   MODELDEF == `build_catalog(config)`, clean restart rebinds ports). Proxies added
+>   under `tests/sim/optitrack_natnet_emulator/`.
 
 Turn a detected config into a running server.
 
@@ -356,7 +439,13 @@ Turn a detected config into a running server.
   at ~`publishRate`; moving the tracked prim moves the published pose. (A Python
   loopback client can stand in for `natnet_ros2` if the SDK isn't built.)
 
-### Commit 4 — Enable/disable lifecycle
+### Commit 4 — Enable/disable lifecycle  ✅ FOLDED INTO COMMIT 3
+
+> The fresh-instance start/stop state machine (`start_server`/`stop_server`/
+> `toggle_server`/`apply_enabled`) and the restart-cleanliness loopback
+> (`test_server_from_config.py::test_stop_frees_port_for_restart`) shipped with
+> Commit 3 above. Remaining for a follow-up: rebuild-on-config-delta while running,
+> and (optionally) auto-reconciling `serverEnabled` edits from the Property panel.
 
 React to `serverEnabled` flipping at runtime.
 
@@ -481,3 +570,64 @@ sequenced; the rest are "note and handle during implementation."
   `target` relationship).
 - Server (`NatNetUnicastServer`) is unchanged — it stays transport-only and
   consumes `set_model_def_payload()` + `enqueue_mocap_data()` exactly as today.
+
+## Debugging discoveries (in-sim end-to-end bring-up)
+
+Hard-won findings from getting the liveliness sentinel green (Isaac emulator →
+`natnet_ros2` on the robot). Captured so the next agent does not re-derive them.
+
+1. **GIL starvation kills the background data thread inside Kit.** The server's
+   `_data_update_loop` daemon (which `time.sleep`s then sends frames) is reliably
+   starved by Kit's render/physics main loop holding the GIL — frames get enqueued
+   but never transmitted, so the client handshakes but receives nothing. Fix:
+   `NatNetServer.auto_stream` (default `True`) gates that loop; the Isaac wrapper
+   sets `auto_stream = False` in `manager.start_server()` and calls
+   `server.pump_once()` **synchronously from the physics-step callback**
+   (`manager.sample_once()`). Outside Kit (host integration tests, sidecar) the
+   default `auto_stream=True` path still works. Don't "optimize" the pump back into
+   the daemon thread.
+
+2. **The liveliness/system path runs the *standalone Pegasus script*, not a saved
+   scene USD.** `tests/conftest.py` sets `ISAAC_SIM_USE_STANDALONE=true` **and
+   `ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py`** — even for
+   `--num-robots 1`. So the NatNet auto-start wiring **must** live in
+   `example_multi_…` (it does); `example_one_…` and the default
+   `airstack up isaac-sim` (which uses `ISAAC_SIM_USE_STANDALONE=false` →
+   `run_isaacsim.launch.py` opening `simple_pegasus.scene.usd`) do **not** exercise
+   it. To reproduce the harness manually you must pass all of
+   `AUTOLAUNCH=true ISAAC_SIM_USE_STANDALONE=true
+   ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py
+   LAUNCH_NATNET=true PLAY_SIM_ON_START=true`.
+
+3. **Single-agent body/prim contract.** For `NUM_ROBOTS=1` the multi script names
+   the body `"Drone"` (bare) → target `/World/drone1/base_link` (the multi script's
+   drone prim path is always `drone{i}`, even for one drone). This matches the robot
+   `natnet_config.yaml` body and the liveliness sentinel topic
+   `/robot_1/perception/optitrack/Drone/pose_cov`.
+
+4. **Target prims appear *after* server start.** The Pegasus `base_link` is created
+   on the first Play tick — after `start_server()`. `_sample_cache` therefore stores
+   target *paths* and re-resolves `stage.GetPrimAtPath()` every `sample_once`, so a
+   body goes from "lost" to valid as soon as its prim exists (no `mark_dirty`).
+
+5. **`ss` is unreliable in the Isaac image; use `/proc/net/udp`.** Confirm the
+   server is bound by looking for ports `1510`/`1511` as hex (`05E6`/`05E7`) in
+   `/proc/net/udp` rather than trusting `ss -ulnp` (returns empty here).
+
+6. **First-message budget.** Cold Isaac boots compile shaders; the NatNet sentinel
+   first-message timeout was raised to 120s (`_NATNET_FIRST_MSG_TIMEOUT`) to match
+   the generous `/clock` (600s) / sentinel-node (300s) budgets. Once streaming, the
+   topic runs at ~98–140 Hz (100 Hz `publishRate`).
+
+7. **The client must retry the connect — the server starts ~100s after the robot.**
+   This was the actual liveliness blocker. `natnet_ros2_node` originally called
+   `connect_and_setup()` **once** in its constructor; on `NetworkError` it logged and
+   gave up. In the standalone path the Isaac emulator only binds 1510/1511 ~100s into
+   sim boot, long after the robot's node starts, so the one-shot connect failed
+   permanently (topic advertised, zero frames). Fix: `connect_and_setup()` returns
+   `bool` and a 2s `connect_timer_` retries the handshake until the first success,
+   then cancels itself (also correct for real Motive powered on after the drone).
+   Verified by starting Isaac and the node together: the node retries for ~90s, then
+   logs "Frame callback registered" the instant the server binds. **To reproduce the
+   race manually, start the robot node first, then Isaac** — bringing the robot up
+   *after* Isaac is ready hides the bug.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
index 2bcfbfad3..462846525 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
@@ -30,6 +30,7 @@
 ATTR_DATA_PORT = "natnet:dataPort"
 ATTR_PUBLISH_RATE = "natnet:publishRate"
 ATTR_NATNET_VERSION = "natnet:natnetVersion"
+ATTR_UP_AXIS = "natnet:upAxis"
 
 BODY_PREFIX = "natnet:body:"
 BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName"
@@ -39,6 +40,13 @@
 
 VALID_MODES = ("unicast", "multicast")
 
+# Streamed up-axis. Motive exposes an "Up Axis" setting; the reference natnet_ros2
+# driver requires it set to Z and passes coordinates through untouched. Isaac Sim /
+# USD is natively Z-up, so "Z" is a pass-through that matches the rest of the
+# AirStack stack (default). "Y" emulates a default (Y-up) Motive by rotating the
+# streamed pose -90deg about X. See ``frames.to_motive_pose``.
+VALID_UP_AXES = ("Y", "Z")
+
 # defaults shared with NatNetUnicastServer
 DEFAULT_SERVER_IP = "172.31.0.200"
 DEFAULT_MULTICAST_ADDR = "239.255.42.99"
@@ -46,6 +54,7 @@
 DEFAULT_DATA_PORT = 1511
 DEFAULT_PUBLISH_RATE = 100.0
 DEFAULT_NATNET_VERSION = "4.4.0.0"
+DEFAULT_UP_AXIS = "Z"
 
 
 def body_attr_name(key: str, field_name: str) -> str:
@@ -137,6 +146,7 @@ class NatNetInterfaceConfig:
     data_port: int = DEFAULT_DATA_PORT
     publish_rate: float = DEFAULT_PUBLISH_RATE
     natnet_version: str = DEFAULT_NATNET_VERSION
+    up_axis: str = DEFAULT_UP_AXIS
     bodies: list[BodyBinding] = field(default_factory=list)
 
     @classmethod
@@ -151,6 +161,7 @@ def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig":
             data_port=int(d.get("data_port", DEFAULT_DATA_PORT)),
             publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)),
             natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)),
+            up_axis=str(d.get("up_axis", DEFAULT_UP_AXIS)).upper(),
             bodies=_normalize_bodies(d.get("bodies")),
         )
 
@@ -164,6 +175,7 @@ def to_dict(self) -> dict[str, Any]:
             "data_port": self.data_port,
             "publish_rate": self.publish_rate,
             "natnet_version": self.natnet_version,
+            "up_axis": self.up_axis,
             "bodies": [b.to_dict() for b in self.bodies],
         }
 
@@ -172,6 +184,8 @@ def validate(self) -> "NatNetInterfaceConfig":
         errors: list[str] = []
         if self.mode not in VALID_MODES:
             errors.append(f"mode must be one of {VALID_MODES}, got {self.mode!r}")
+        if str(self.up_axis).upper() not in VALID_UP_AXES:
+            errors.append(f"up_axis must be one of {VALID_UP_AXES}, got {self.up_axis!r}")
         for port_name, port in (("command_port", self.command_port), ("data_port", self.data_port)):
             if not (0 < port < 65536):
                 errors.append(f"{port_name} must be in 1..65535, got {port}")
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
index 4902fb93c..c767733b8 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
@@ -6,10 +6,12 @@
 prim world poses become an ``sFrameOfMocapData`` of rigid bodies that the server
 streams to ``natnet_ros2``.
 
-**Frame convention:** AirStack's ``natnet_ros2`` copies the rigid-body pose straight
-through (``rb_to_pose`` is an identity copy) and nothing downstream re-axes it, so we
-emit the prim's USD world pose **as-is** (no Z-up->Y-up swap). Swapping here would
-desync the published pose from the rest of the stack.
+**Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2``
+(and the reference driver) require it set to **Z** and copy the rigid-body pose
+straight through (``rb_to_pose`` is an identity copy). Isaac Sim / USD is natively
+Z-up, so the default ``up_axis="Z"`` emits the prim's USD world pose **as-is** and
+stays in sync with the rest of the stack. ``up_axis="Y"`` emulates a default (Y-up)
+Motive — see :func:`to_motive_pose`.
 
 **params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``):
 ``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it.
@@ -44,6 +46,29 @@ def lost(cls, streaming_id: int) -> "BodySample":
         return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False)
 
 
+def to_motive_pose(position, orientation, up_axis: str = "Z"):
+    """Re-express an Isaac (Z-up) world pose in Motive's streamed up-axis frame.
+
+    Returns ``(position, orientation)`` re-axed for the given ``up_axis``:
+
+    - ``"Z"`` (default) — identity pass-through. Isaac/USD is Z-up and the
+      reference Motive setup streams Z-up, so the pose flows through unchanged and
+      matches ``natnet_ros2`` (which does no axis conversion).
+    - ``"Y"`` — emulate a default Y-up Motive by rotating the pose -90 deg about X
+      (Isaac ``+Z`` -> Motive ``+Y``): ``(x, y, z) -> (x, z, -y)``. This is a
+      proper right-handed -> right-handed change of basis (det = +1), so the
+      quaternion's vector part takes the same swap and the scalar part is
+      unchanged: ``(qx, qy, qz, qw) -> (qx, qz, -qy, qw)``.
+
+    Non-finite components (a lost body's NaN position) pass through unchanged.
+    """
+    if str(up_axis).upper() != "Y":
+        return position, orientation
+    x, y, z = position
+    qx, qy, qz, qw = orientation
+    return (x, z, -y), (qx, qz, -qy, qw)
+
+
 def make_rigid_body_data(sample: BodySample) -> sRigidBodyData:
     """Build one ``sRigidBodyData`` from a sample (sets the tracking-valid bit)."""
     rb = sRigidBodyData()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
index 197ed3b2a..fb9699cb1 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -17,8 +17,8 @@
 from __future__ import annotations
 
 from .catalog import build_catalog, find_duplicate_targets
-from .config import NatNetInterfaceConfig
-from .frames import BodySample, build_frame
+from .config import DEFAULT_UP_AXIS, NatNetInterfaceConfig
+from .frames import BodySample, build_frame, to_motive_pose
 from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets
 
 
@@ -67,6 +67,7 @@ def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str:
     lines.append(f"  dataPort      : {cfg.data_port}")
     lines.append(f"  publishRate   : {cfg.publish_rate}")
     lines.append(f"  natnetVersion : {cfg.natnet_version}")
+    lines.append(f"  upAxis        : {cfg.up_axis}")
     if cfg.bodies:
         lines.append(f"  bodies ({len(cfg.bodies)}):")
         for b in cfg.bodies:
@@ -98,6 +99,10 @@ def __init__(self, server_factory=None):
         self._frame_counter = 0
         self._catalog_signature = None
         self._physx_sub = None
+        # Streamed up-axis (from the interface config; re-read on every resync).
+        # "Z" (default) streams the Isaac/USD world pose as-is; "Y" re-axes it to
+        # emulate a default Y-up Motive. See frames.to_motive_pose.
+        self._up_axis = DEFAULT_UP_AXIS
 
     # --- lifecycle -------------------------------------------------------------
 
@@ -277,6 +282,7 @@ def _resync(self, stage) -> bool:
             self._sample_cache = []
             return False
         config = read_interface(interfaces[0])
+        self._up_axis = config.up_axis
         if self._server is not None:
             self._server.set_model_def_payload(build_catalog(config).pack())
         # Cache target *paths* (not prim handles): the prim is re-resolved every
@@ -317,7 +323,7 @@ def sample_once(self, stage=None):
             if pose is None:
                 samples.append(BodySample.lost(streaming_id))
             else:
-                position, orientation = pose
+                position, orientation = to_motive_pose(*pose, up_axis=self._up_axis)
                 samples.append(BodySample(streaming_id, position, orientation, valid=True))
 
         frame = build_frame(
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
index b5672a87b..1844595dd 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
@@ -22,6 +22,7 @@
     DEFAULT_DATA_PORT,
     DEFAULT_PUBLISH_RATE,
     DEFAULT_SERVER_IP,
+    DEFAULT_UP_AXIS,
     BodyBinding,
     NatNetInterfaceConfig,
 )
@@ -42,6 +43,7 @@ def build_drone_config(
     data_port: int = DEFAULT_DATA_PORT,
     publish_rate: float = DEFAULT_PUBLISH_RATE,
     server_enabled: bool = True,
+    up_axis: str = DEFAULT_UP_AXIS,
 ) -> NatNetInterfaceConfig:
     """Build a validated config with one rigid body per drone.
 
@@ -65,6 +67,7 @@ def build_drone_config(
         command_port=command_port,
         data_port=data_port,
         publish_rate=publish_rate,
+        up_axis=up_axis,
         bodies=bodies,
     )
     cfg.validate()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index 246670cac..656d8b88e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -30,7 +30,7 @@
 
 import omni.ext
 
-from .config import VALID_MODES, BodyBinding, NatNetInterfaceConfig
+from .config import VALID_MODES, VALID_UP_AXES, BodyBinding, NatNetInterfaceConfig
 from .manager import NatNetServerManager
 from .usd_bindings import author_interface, find_interfaces, read_interface, read_world_pose
 
@@ -193,10 +193,11 @@ def _build_window(self):
                 ui.Separator(height=6)
                 self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled)
                 self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip)
-                self._combo_row(ui, "Mode", "mode", self._cfg.mode)
+                self._combo_row(ui, "Mode", "mode", self._cfg.mode, VALID_MODES)
                 self._int_row(ui, "Command port", "command_port", self._cfg.command_port)
                 self._int_row(ui, "Data port", "data_port", self._cfg.data_port)
                 self._float_row(ui, "Publish rate (Hz)", "publish_rate", self._cfg.publish_rate)
+                self._combo_row(ui, "Up axis", "up_axis", self._cfg.up_axis, VALID_UP_AXES)
 
                 ui.Separator(height=6)
                 ui.Label("Tracked bodies", height=0, style={"font_size": 14})
@@ -249,13 +250,13 @@ def _float_row(self, ui, label, key, value):
                 lambda m, k=key: self._set_cfg_field(k, m.get_value_as_float())
             )
 
-    def _combo_row(self, ui, label, key, value):
+    def _combo_row(self, ui, label, key, value, choices):
         with ui.HStack(height=0):
             ui.Label(label, width=_LABEL_WIDTH)
-            index = VALID_MODES.index(value) if value in VALID_MODES else 0
-            combo = ui.ComboBox(index, *VALID_MODES)
+            index = choices.index(value) if value in choices else 0
+            combo = ui.ComboBox(index, *choices)
             combo.model.get_item_value_model().add_value_changed_fn(
-                lambda m: self._set_cfg_field("mode", VALID_MODES[m.get_value_as_int()])
+                lambda m, k=key, c=choices: self._set_cfg_field(k, c[m.get_value_as_int()])
             )
 
     def _set_cfg_field(self, attr, value):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
index 7fd193c80..3fddc4f58 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -25,6 +25,7 @@
     ATTR_PUBLISH_RATE,
     ATTR_SERVER_ENABLED,
     ATTR_SERVER_IP,
+    ATTR_UP_AXIS,
     BODY_FIELD_PARENT_ID,
     BODY_FIELD_RIGID_BODY_NAME,
     BODY_FIELD_STREAMING_ID,
@@ -36,6 +37,7 @@
     DEFAULT_NATNET_VERSION,
     DEFAULT_PUBLISH_RATE,
     DEFAULT_SERVER_IP,
+    DEFAULT_UP_AXIS,
     MARKER_ATTR,
     BodyBinding,
     NatNetInterfaceConfig,
@@ -69,6 +71,7 @@ def author_interface(stage, prim_path: str, config: Any) -> Any:
     _set(prim, ATTR_DATA_PORT, Sdf.ValueTypeNames.Int, cfg.data_port)
     _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate)
     _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version)
+    _set(prim, ATTR_UP_AXIS, Sdf.ValueTypeNames.Token, cfg.up_axis)
 
     for key, body in cfg.assign_instance_keys():
         _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name)
@@ -94,6 +97,7 @@ def read_interface(prim) -> NatNetInterfaceConfig:
         data_port=int(_get(prim, ATTR_DATA_PORT, DEFAULT_DATA_PORT)),
         publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)),
         natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)),
+        up_axis=str(_get(prim, ATTR_UP_AXIS, DEFAULT_UP_AXIS)),
         bodies=_read_bodies(prim),
     )
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
index 8bbab1809..e26165d99 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
@@ -15,11 +15,34 @@
     BodySample,
     build_frame,
     make_rigid_body_data,
+    to_motive_pose,
 )
 
 pytestmark = pytest.mark.unit
 
 
+def test_to_motive_pose_z_is_identity():
+    pos = (1.0, 2.0, 3.0)
+    quat = (0.1, 0.2, 0.3, 0.9)
+    assert to_motive_pose(pos, quat, up_axis="Z") == (pos, quat)
+    # Case-insensitive and default to Z.
+    assert to_motive_pose(pos, quat, up_axis="z") == (pos, quat)
+    assert to_motive_pose(pos, quat) == (pos, quat)
+
+
+def test_to_motive_pose_y_swaps_axes_and_quat():
+    # (x, y, z) -> (x, z, -y); quat vector part takes the same swap, scalar kept.
+    pos, quat = to_motive_pose((1.0, 2.0, 3.0), (0.1, 0.2, 0.3, 0.9), up_axis="Y")
+    assert pos == (1.0, 3.0, -2.0)
+    assert quat == (0.1, 0.3, -0.2, 0.9)
+
+
+def test_to_motive_pose_y_maps_isaac_up_to_motive_up():
+    # Isaac +Z (up) must become Motive +Y (up) under the Y-up emulation.
+    pos, _ = to_motive_pose((0.0, 0.0, 1.0), (0.0, 0.0, 0.0, 1.0), up_axis="y")
+    assert pos == (0.0, 1.0, 0.0)
+
+
 def test_make_rigid_body_data_copies_pose_and_sets_valid_bit():
     rb = make_rigid_body_data(
         BodySample(7, (1.0, 2.0, 3.0), (0.0, 0.0, 0.7071068, 0.7071068), valid=True)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
index 0807b70cd..9546ad9dc 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
@@ -84,6 +84,18 @@ def test_reauthoring_removes_stale_bodies():
     assert [b.rigid_body_name for b in cfg.bodies] == ["Drone"]
 
 
+def test_up_axis_authors_and_reads_back():
+    stage = _new_stage()
+    # Default (absent) -> Z.
+    author_interface(stage, "/World/NatNetInterface", _CONFIG)
+    assert read_interface(find_interfaces(stage)[0]).up_axis == "Z"
+
+    # Explicit Y survives the USD round trip.
+    cfg = NatNetInterfaceConfig.from_dict({**_CONFIG, "up_axis": "Y"})
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    assert read_interface(find_interfaces(stage)[0]).up_axis == "Y"
+
+
 def test_empty_target_round_trips():
     # The UI's "Add body" can create a body with no target yet (set later in the
     # Property panel); it must author and read back cleanly with an empty target.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
index c015c1a4e..ddfcd5705 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
@@ -27,9 +27,22 @@ def test_defaults_match_server_expectations():
     assert cfg.mode == "unicast"
     assert cfg.command_port == 1510
     assert cfg.data_port == 1511
+    assert cfg.up_axis == "Z"  # Isaac/USD native; matches the reference Motive setup
     assert cfg.bodies == []
 
 
+def test_up_axis_from_dict_normalizes_case():
+    assert NatNetInterfaceConfig.from_dict({"up_axis": "y"}).up_axis == "Y"
+    assert NatNetInterfaceConfig.from_dict({"up_axis": "z"}).up_axis == "Z"
+    # Absent -> default Z.
+    assert NatNetInterfaceConfig.from_dict({}).up_axis == "Z"
+
+
+def test_up_axis_survives_round_trip():
+    cfg = NatNetInterfaceConfig.from_dict({"up_axis": "Y"})
+    assert NatNetInterfaceConfig.from_dict(cfg.to_dict()).up_axis == "Y"
+
+
 def test_from_dict_with_bodies_as_list():
     cfg = NatNetInterfaceConfig.from_dict(
         {
@@ -111,6 +124,8 @@ def test_assign_instance_keys_are_unique():
         {"data_port": 70000},
         {"command_port": 1510, "data_port": 1510},
         {"publish_rate": 0},
+        {"up_axis": "X"},
+        {"up_axis": "bogus"},
     ],
 )
 def test_validate_rejects_bad_server_config(overrides):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
index ac31c22b4..fa13a7eb2 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
@@ -137,6 +137,35 @@ def test_moving_prim_updates_streamed_position():
     assert round(frame.RigidBodies[0].x, 3) == 10.0
 
 
+def test_up_axis_z_streams_isaac_pose_as_is():
+    stage = Usd.Stage.CreateInMemory()
+    _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0))
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", up_axis="Z", bodies=[BodyBinding("Drone", "/World/base_link", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, _fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    rb = mgr.sample_once(stage).RigidBodies[0]
+    assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 2.0, 3.0)
+
+
+def test_up_axis_y_reaxes_streamed_pose():
+    # Y-up Motive emulation: Isaac (x, y, z) streams as (x, z, -y).
+    stage = Usd.Stage.CreateInMemory()
+    _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0))
+    cfg = NatNetInterfaceConfig(
+        server_ip="127.0.0.1", up_axis="Y", bodies=[BodyBinding("Drone", "/World/base_link", 1)]
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    mgr, _fake = _manager_with_fake()
+    mgr.start_server(cfg)
+
+    rb = mgr.sample_once(stage).RigidBodies[0]
+    assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 3.0, -2.0)
+
+
 def test_body_added_while_live_is_picked_up_on_resync():
     stage = Usd.Stage.CreateInMemory()
     _xform(stage, "/World/a", translate=(1.0, 0.0, 0.0))

From 58505285d619f129652f15617f1c0168e3190309 Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Fri, 12 Jun 2026 00:21:44 -0400
Subject: [PATCH 19/27] feat(natnet): wire MAVROS vision_pose path and PX4
 vision SITL profile

Remap vision_pose_converter to the interface/mavros namespace, install
mavros_extras in the robot image, and add layered PX4_PARAM_PROFILE env
files so Isaac SITL can fuse NatNet external vision (EKF2_EV_CTRL, no GPS,
indoor mag disabled). Document the vision profile and provide an overrides
bundle for NatNet + vision sim bring-up.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .env                                          |  5 +++
 .../isaac_sim/pegasus_scene_setup.md          | 16 +++++++++
 gcs/docker/gcs-base-docker-compose.yaml       |  2 ++
 overrides/isaac-natnet-vision.env             |  9 +++++
 robot/docker/Dockerfile.robot                 |  2 ++
 .../src/perception/natnet_ros2/README.md      |  9 ++---
 .../launch/vision_pose_converter.launch.xml   | 10 +++---
 .../isaac-sim/docker/docker-compose.yaml      |  1 +
 .../isaac-sim/docker/px4-profiles/default.env |  7 ++++
 .../isaac-sim/docker/px4-profiles/vision.env  | 35 +++++++++++++++++++
 .../emulator/server/natnet_unicast_server.py  |  1 -
 11 files changed, 87 insertions(+), 10 deletions(-)
 create mode 100644 overrides/isaac-natnet-vision.env
 create mode 100644 simulation/isaac-sim/docker/px4-profiles/default.env
 create mode 100644 simulation/isaac-sim/docker/px4-profiles/vision.env

diff --git a/.env b/.env
index 0d994bc51..2633b1f8e 100644
--- a/.env
+++ b/.env
@@ -51,6 +51,11 @@ URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
 
 DEBUG_RVIZ="false"  # "true" or "false". If true, launches RViz alongside the robot via desktop_bringup/robot.launch.xml.
 
+LAUNCH_NATNET="false"
+# PX4 SITL param profile for isaac-sim (simulation/isaac-sim/docker/px4-profiles/).
+# Use "vision" with LAUNCH_NATNET + MAVROS vision_pose; "default" for plain Pegasus SITL.
+PX4_PARAM_PROFILE="default"
+
 # offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication. 
 OFFBOARD_BASE_PORT=14540
 ONBOARD_BASE_PORT=14580
diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md
index da4293dc7..72b3bdc97 100644
--- a/docs/simulation/isaac_sim/pegasus_scene_setup.md
+++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md
@@ -165,6 +165,22 @@ PX4_RENDERING_HZ="30"
 - **`PX4_PHYSICS_HZ`** — Sets `physics_dt = 1 / PX4_PHYSICS_HZ` in Isaac Sim's physics scene, and automatically syncs PX4's `IMU_INTEG_RATE` parameter to the same value via `PX4LaunchTool` → `px4-rc.simulator`. Patched within the Docker image to read the environment variable and set the IMU_INTEG_RATE parameter.
 - **`PX4_RENDERING_HZ`** — Sets the rendering frame rate independently of physics. 30 Hz rendering has no effect on physics accuracy or PX4 behavior, but does slightly affect performance due to resource usage.
 
+### PX4 parameter profiles (external vision)
+
+Isaac Sim loads a layered PX4 SITL parameter profile from `simulation/isaac-sim/docker/px4-profiles/`. Select the profile with `PX4_PARAM_PROFILE` in the top-level `.env`:
+
+```bash
+# Plain Pegasus SITL
+PX4_PARAM_PROFILE="default"
+
+# NatNet / mocap → MAVROS vision_pose → EKF2 external vision
+PX4_PARAM_PROFILE="vision"
+```
+
+Profiles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `vision` with robot-side `LAUNCH_NATNET=true` and `publish_to_mavros: true` in `natnet_config.yaml`.
+
+Convenience bundle: `airstack up --env-file overrides/isaac-natnet-vision.env`.
+
 ### Valid values
 
 PX4's documented presets for `IMU_INTEG_RATE` are **100, 200, 250, 400 Hz**. The minimum is **100 Hz** — the EKF2 estimator runs at 100 Hz (10 ms period) and `IMU_INTEG_RATE` must be at least this fast. Values below 100 Hz are accepted by the firmware but cause attitude controller oscillation and are not recommended.
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index c8ac5200e..4fe87155a 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -36,6 +36,8 @@ services:
       - NVIDIA_DRIVER_CAPABILITIES=all
       # Number of robots (used by action_relay to spawn per-robot relays)
       - NUM_ROBOTS=${NUM_ROBOTS:-1}
+      # Launching NatNet for Optitrack Motion Capture
+      - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
       # Record bags
       - RECORD_BAGS=${RECORD_BAGS}
     image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env
new file mode 100644
index 000000000..d085175c9
--- /dev/null
+++ b/overrides/isaac-natnet-vision.env
@@ -0,0 +1,9 @@
+# Isaac Sim + NatNet mocap with PX4 external-vision fusion.
+# Usage: airstack up --env-file overrides/isaac-natnet-vision.env
+#
+# Root .env is always loaded first; this file overrides selected keys.
+
+LAUNCH_NATNET=true
+PX4_PARAM_PROFILE=vision
+ISAAC_SIM_USE_STANDALONE=true
+ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index 894874961..f6e48b150 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -96,6 +96,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta
 RUN apt update -y && apt install -y --no-install-recommends \
   ros-dev-tools \
   ros-${ROS_DISTRO}-mavros \
+  ros-${ROS_DISTRO}-mavros-extras \
   ros-${ROS_DISTRO}-tf2* \
   ros-${ROS_DISTRO}-stereo-image-proc \
   ros-${ROS_DISTRO}-image-view \
@@ -328,6 +329,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta
 RUN apt update -y && apt install -y --no-install-recommends \
   ros-dev-tools \
   ros-${ROS_DISTRO}-mavros \
+  ros-${ROS_DISTRO}-mavros-extras \
   ros-${ROS_DISTRO}-tf2* \
   ros-${ROS_DISTRO}-stereo-image-proc \
   ros-${ROS_DISTRO}-image-view \
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index 502525071..220cdf7c6 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -30,8 +30,8 @@ NatNet ROS 2 Node
     ├→ /robot_1/perception/optitrack/{body_name}/pose_cov  (PoseWithCovarianceStamped, always)
     └→ (Optional, publish_to_mavros: true)
          vision_pose_converter_node
-           ├→ /robot_1/mavros/vision_pose/pose
-           └→ /robot_1/mavros/vision_pose/pose_cov
+           ├→ /robot_1/interface/mavros/vision_pose/pose
+           └→ /robot_1/interface/mavros/vision_pose/pose_cov
 ```
 
 ## Interfaces
@@ -62,9 +62,10 @@ For each tracked rigid body `{body_name}` from Motive:
 
 When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4:
 
-- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message)
-- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized)
+- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message)
+- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized)
 - **Enabled by**: `publish_to_mavros: true` in config
+- **PX4 side**: set `PX4_PARAM_PROFILE=vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/px4-profiles/vision.env`
 
 ## Configuration
 
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
index aad9c4474..cc15983fd 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
@@ -12,9 +12,9 @@
     Input  (from NatNet node):
       /{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov
 
-    Output (consumed by MAVROS):
-      /{ROBOT_NAME}/mavros/vision_pose/pose
-      /{ROBOT_NAME}/mavros/vision_pose/pose_cov
+    Output (consumed by MAVROS under interface bringup namespace):
+      /{ROBOT_NAME}/interface/mavros/vision_pose/pose
+      /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov
   -->
 
   <arg name="config_file"
@@ -35,9 +35,9 @@
     <remap from="input_pose"
            to="/$(env ROBOT_NAME robot_1)/perception/optitrack/$(var body_name)/pose_cov"/>
     <remap from="output_pose"
-           to="/$(env ROBOT_NAME robot_1)/mavros/vision_pose/pose"/>
+           to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose"/>
     <remap from="output_pose_cov"
-           to="/$(env ROBOT_NAME robot_1)/mavros/vision_pose/pose_cov"/>
+           to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose_cov"/>
 
     <param name="use_sim_time" value="$(var use_sim_time)"/>
   </node>
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index c37e8cb66..4ba91f58a 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -33,6 +33,7 @@ services:
         ipv4_address: 172.31.0.200 # required to not conflict with other default docker networks on the host machine
     env_file:
       - ./omni_pass.env
+      - ./px4-profiles/${PX4_PARAM_PROFILE:-default}.env
     environment:
       - AUTOLAUNCH=${AUTOLAUNCH:-'false'}
       - DISPLAY=${DISPLAY}
diff --git a/simulation/isaac-sim/docker/px4-profiles/default.env b/simulation/isaac-sim/docker/px4-profiles/default.env
new file mode 100644
index 000000000..b9b3f54bc
--- /dev/null
+++ b/simulation/isaac-sim/docker/px4-profiles/default.env
@@ -0,0 +1,7 @@
+# PX4 SITL parameter profile: default (no overrides beyond airframe + image patches).
+#
+# Selected by PX4_PARAM_PROFILE in the top-level .env (compose interpolation).
+# Variables here are injected into the isaac-sim container and applied at PX4 boot
+# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS.
+#
+# See vision.env for external-vision / NatNet → MAVROS fusion tuning.
diff --git a/simulation/isaac-sim/docker/px4-profiles/vision.env b/simulation/isaac-sim/docker/px4-profiles/vision.env
new file mode 100644
index 000000000..83dacd113
--- /dev/null
+++ b/simulation/isaac-sim/docker/px4-profiles/vision.env
@@ -0,0 +1,35 @@
+# PX4 SITL parameter profile: external vision or state estimation.
+#
+# Data path:
+#   Isaac NatNet emulator → natnet_ros2 → vision_pose_converter
+#   → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2
+#
+# Pair with robot-side publish_to_mavros: true in natnet_config.yaml.
+# Set PX4_PARAM_PROFILE=vision in .env (or overrides/isaac-natnet-vision.env).
+
+# Fuse horizontal position, vertical position, and yaw from VISION_POSITION_ESTIMATE.
+# Bit 2 (velocity) omitted — pose_cov has no twist; MAVROS vision_pose is pose-only.
+PX4_PARAM_EKF2_EV_CTRL=11
+
+# Use vision as the height reference (indoor mocap; no GNSS in Isaac SITL).
+PX4_PARAM_EKF2_HGT_REF=3
+
+# No GPS receiver in sim; avoid waiting for GNSS aiding.
+PX4_PARAM_EKF2_GPS_CTRL=0
+
+# Trust covariance from vision_pose_cov (natnet_config position/orientation matrices).
+PX4_PARAM_EKF2_EV_NOISE_MD=0
+
+# End-to-end delay: NatNet frame + ROS + MAVROS (milliseconds). Tune if fusion lags.
+PX4_PARAM_EKF2_EV_DELAY=20
+
+# Mocap tracks base_link at the vehicle CG — no lever arm.
+PX4_PARAM_EKF2_EV_POS_X=0
+PX4_PARAM_EKF2_EV_POS_Y=0
+PX4_PARAM_EKF2_EV_POS_Z=0
+
+# Allow arming without GPS fix when fusing external vision only.
+PX4_PARAM_COM_ARM_WO_GPS=1
+
+# Indoor vision yaw — don't fuse simulated magnetometer
+PX4_PARAM_EKF2_MAG_TYPE=5
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index fdef63b7f..f863ca087 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -65,7 +65,6 @@ def _command_listener_loop(self):
                 data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests
                 if not data:
                     continue
-                print(f"[Command Listener] Received command request from client {addr}.")
                 self._handle_command_request(data, addr)
             except Exception as e:
                 if self.shutdown_event.is_set():

From d9a552b6a0be466e23067e4cd37e1269ada15cea Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Fri, 12 Jun 2026 00:21:49 -0400
Subject: [PATCH 20/27] feat(natnet): add guarded synthetic GPS origin for
 mocap arming

PX4 with EKF2_GPS_CTRL=0 has valid local vision fusion but no global
position, so AUTO.LOITER preflight fails until an origin is set. Add
mavros_gp_origin_node to publish a one-shot set_gp_origin after MAVROS
connects, skipping when a real origin already exists. Launched alongside
vision_pose_converter when publish_to_mavros is enabled.

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .../src/perception/natnet_ros2/CMakeLists.txt |   1 +
 .../src/perception/natnet_ros2/README.md      |  18 +++
 .../natnet_ros2/config/mavros_gp_origin.yaml  |  18 +++
 .../launch/mavros_gp_origin.launch.xml        |  34 +++++
 .../natnet_ros2/launch/natnet_ros2.launch.py  |  21 ++-
 .../src/perception/natnet_ros2/package.xml    |   1 +
 .../natnet_ros2/src/mavros_gp_origin_node.py  | 123 ++++++++++++++++++
 7 files changed, 215 insertions(+), 1 deletion(-)
 create mode 100644 robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml
 create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
 create mode 100755 robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py

diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
index ff47a00da..4b72ed3d2 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
+++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
@@ -69,6 +69,7 @@ endif()
 # ---------------------------------------------------------------------------
 install(PROGRAMS
   src/vision_pose_converter_node.py
+  src/mavros_gp_origin_node.py
   DESTINATION lib/${PROJECT_NAME})
 
 # ---------------------------------------------------------------------------
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index 220cdf7c6..2fb5b69cd 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -29,6 +29,8 @@ NatNet ROS 2 Node
     ├→ /robot_1/perception/optitrack/{body_name}           (PoseStamped, optional)
     ├→ /robot_1/perception/optitrack/{body_name}/pose_cov  (PoseWithCovarianceStamped, always)
     └→ (Optional, publish_to_mavros: true)
+         mavros_gp_origin_node
+           └→ /robot_1/interface/mavros/global_position/set_gp_origin  (once, guarded)
          vision_pose_converter_node
            ├→ /robot_1/interface/mavros/vision_pose/pose
            └→ /robot_1/interface/mavros/vision_pose/pose_cov
@@ -67,6 +69,22 @@ When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose
 - **Enabled by**: `publish_to_mavros: true` in config
 - **PX4 side**: set `PX4_PARAM_PROFILE=vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/px4-profiles/vision.env`
 
+##### Synthetic GPS origin (mocap / no-GNSS arming)
+
+With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fuses vision into a valid *local*
+position but has **no global position**. Modes that require one — such as
+`AUTO.LOITER` — then fail preflight and refuse to arm (`Arming denied: Resolve
+system health failures first`). When `publish_to_mavros: true`,
+`mavros_gp_origin_node` publishes a synthetic origin once at startup:
+
+- **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped`
+- **Guarded**: waits for `mavros/state.connected`, then publishes only if no
+  origin already exists (it watches `…/global_position/gp_origin`), so a
+  GNSS-equipped vehicle is left untouched.
+- **Params** (`config/mavros_gp_origin.yaml`): `enabled` (default `true`),
+  `latitude/longitude/altitude` (default PX4 SITL home, Zurich), `settle_sec`.
+  Set `enabled: false` to rely on real GNSS.
+
 ## Configuration
 
 Edit `config/natnet_config.yaml`:
diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml
new file mode 100644
index 000000000..b5edc3f70
--- /dev/null
+++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml
@@ -0,0 +1,18 @@
+# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS.
+# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled.
+
+/**:
+  ros__parameters:
+    # With GNSS disabled, PX4 has no global position, so modes that require one
+    # (e.g. AUTO.LOITER) refuse to arm. Setting an origin lets PX4 derive a
+    # global position from the fused vision estimate. Guarded: skipped if an
+    # origin already exists (e.g. on a GNSS-equipped vehicle).
+    enabled: true
+    # Default matches PX4 SITL's default home (Zurich); shared by all robots so
+    # they share one global datum.
+    latitude: 47.397742
+    longitude: 8.545594
+    altitude: 488.0
+    # Wait this long after MAVROS connects (listening for an existing origin)
+    # before publishing the synthetic one.
+    settle_sec: 5.0
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
new file mode 100644
index 000000000..4013b7dda
--- /dev/null
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+  <!--
+    MAVROS GPS Origin Launch File
+
+    Publishes a synthetic GPS origin once at startup for mocap / no-GNSS flight.
+    Included from natnet_ros2.launch.py when publish_to_mavros is true.
+
+    Output (consumed by MAVROS under interface bringup namespace):
+      /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin
+  -->
+
+  <arg name="config_file"
+       default="$(find-pkg-share natnet_ros2)/config/mavros_gp_origin.yaml"
+       description="ROS 2 parameter YAML (/** ros__parameters block)"/>
+
+  <arg name="use_sim_time" default="false"
+       description="Forwarded after YAML so sim stacks can force true"/>
+
+  <node pkg="natnet_ros2" exec="mavros_gp_origin_node.py"
+        name="mavros_gp_origin" output="screen">
+    <param from="$(var config_file)" allow_substs="true"/>
+
+    <remap from="set_gps_origin"
+           to="/$(env ROBOT_NAME robot_1)/interface/mavros/global_position/set_gp_origin"/>
+    <remap from="current_gps_origin"
+           to="/$(env ROBOT_NAME robot_1)/interface/mavros/global_position/gp_origin"/>
+    <remap from="mavros_state"
+           to="/$(env ROBOT_NAME robot_1)/interface/mavros/state"/>
+
+    <param name="use_sim_time" value="$(var use_sim_time)"/>
+  </node>
+
+</launch>
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
index cb7c178d8..98a8b237c 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
@@ -43,14 +43,17 @@ def generate_launch_description() -> LaunchDescription:
     pkg_share = get_package_share_directory('natnet_ros2')
     default_natnet_yaml = os.path.join(pkg_share, 'config', 'natnet_config.yaml')
     default_vp_yaml = os.path.join(pkg_share, 'config', 'vision_pose_converter.yaml')
+    default_gp_origin_yaml = os.path.join(pkg_share, 'config', 'mavros_gp_origin.yaml')
 
     config_file = LaunchConfiguration('config_file')
     vision_pose_config_file = LaunchConfiguration('vision_pose_config_file')
+    gp_origin_config_file = LaunchConfiguration('gp_origin_config_file')
     use_sim_time = LaunchConfiguration('use_sim_time')
 
     def launch_setup(context, *_args, **_kwargs):
         cfg_path = config_file.perform(context)
         vp_path = vision_pose_config_file.perform(context)
+        gp_path = gp_origin_config_file.perform(context)
         ust = use_sim_time.perform(context)
 
         ros_params = _ros_params_from_file(cfg_path)
@@ -80,6 +83,17 @@ def launch_setup(context, *_args, **_kwargs):
         ]
 
         if publish_mavros:
+            actions.append(
+                IncludeLaunchDescription(
+                    FrontendLaunchDescriptionSource(
+                        os.path.join(pkg_share, 'launch', 'mavros_gp_origin.launch.xml'),
+                    ),
+                    launch_arguments=[
+                        ('config_file', gp_path),
+                        ('use_sim_time', ust),
+                    ],
+                ),
+            )
             actions.append(
                 IncludeLaunchDescription(
                     FrontendLaunchDescriptionSource(
@@ -107,10 +121,15 @@ def launch_setup(context, *_args, **_kwargs):
                 default_value=default_vp_yaml,
                 description='vision_pose_converter parameter YAML.',
             ),
+            DeclareLaunchArgument(
+                'gp_origin_config_file',
+                default_value=default_gp_origin_yaml,
+                description='mavros_gp_origin parameter YAML.',
+            ),
             DeclareLaunchArgument(
                 'use_sim_time',
                 default_value='false',
-                description='Forwarded to vision_pose_converter.launch.xml.',
+                description='Forwarded to MAVROS bridge launch files.',
             ),
             OpaqueFunction(function=launch_setup),
         ],
diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml
index f9632b0f7..621469145 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/package.xml
+++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml
@@ -26,6 +26,7 @@
 
   <!-- Optional: MAVROS integration -->
   <exec_depend>mavros_msgs</exec_depend>
+  <exec_depend>geographic_msgs</exec_depend>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>launch</exec_depend>
diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py
new file mode 100755
index 000000000..d9b1c3754
--- /dev/null
+++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py
@@ -0,0 +1,123 @@
+#!/usr/bin/env python3
+
+"""
+MAVROS GPS Origin Node
+
+Publishes a synthetic GPS origin to MAVROS once at startup for mocap / no-GNSS
+flight. With GNSS disabled, PX4 fuses vision into a valid local position but
+has no global position, so modes that require one (e.g. AUTO.LOITER) refuse to
+arm. Setting an origin lets PX4 derive global position from the fused estimate.
+
+The publish is guarded: it waits for MAVROS to connect, watches for an existing
+origin, and only publishes if none is present — GNSS-equipped vehicles are left
+untouched.
+"""
+
+import rclpy
+from rclpy.node import Node
+from geographic_msgs.msg import GeoPointStamped
+from mavros_msgs.msg import State
+
+
+class MavrosGpOriginNode(Node):
+    """One-shot synthetic GPS origin publisher for MAVROS / PX4."""
+
+    def __init__(self):
+        super().__init__('mavros_gp_origin')
+
+        self.declare_parameter('enabled', True)
+        self.declare_parameter('latitude', 47.397742)
+        self.declare_parameter('longitude', 8.545594)
+        self.declare_parameter('altitude', 488.0)
+        # Seconds to wait after MAVROS connects (listening for an existing
+        # origin) before publishing our synthetic one.
+        self.declare_parameter('settle_sec', 5.0)
+
+        self._enabled = self.get_parameter('enabled').value
+        if not self._enabled:
+            self.get_logger().info('Synthetic GPS origin disabled (enabled=false).')
+            return
+
+        self._lat = self.get_parameter('latitude').value
+        self._lon = self.get_parameter('longitude').value
+        self._alt = self.get_parameter('altitude').value
+        self._settle_sec = self.get_parameter('settle_sec').value
+
+        self._done = False
+        self._origin_exists = False
+        self._connected_since = None
+        self._publish_count = 0
+
+        self._set_origin_pub = self.create_publisher(
+            GeoPointStamped, 'set_gps_origin', 10
+        )
+        self._origin_sub = self.create_subscription(
+            GeoPointStamped, 'current_gps_origin', self._on_existing_origin, 10
+        )
+        self._state_sub = self.create_subscription(
+            State, 'mavros_state', self._on_mavros_state, 10
+        )
+        self._timer = self.create_timer(1.0, self._tick)
+
+        self.get_logger().info(
+            f'MAVROS GPS origin node started '
+            f'(lat={self._lat}, lon={self._lon}, alt={self._alt}, '
+            f'settle_sec={self._settle_sec})'
+        )
+
+    def _on_existing_origin(self, _msg: GeoPointStamped):
+        """An origin already exists (e.g. from GNSS) — never override it."""
+        if not self._origin_exists and not self._done:
+            self.get_logger().info(
+                'Existing GPS origin detected; skipping synthetic origin.'
+            )
+        self._origin_exists = True
+
+    def _on_mavros_state(self, msg: State):
+        if msg.connected and self._connected_since is None:
+            self._connected_since = self.get_clock().now()
+
+    def _tick(self):
+        if self._done:
+            return
+        if self._origin_exists:
+            self._done = True
+            self._timer.cancel()
+            return
+        if self._connected_since is None:
+            return
+        elapsed = (self.get_clock().now() - self._connected_since).nanoseconds * 1e-9
+        if elapsed < self._settle_sec:
+            return
+
+        msg = GeoPointStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.position.latitude = self._lat
+        msg.position.longitude = self._lon
+        msg.position.altitude = self._alt
+        self._set_origin_pub.publish(msg)
+        self._publish_count += 1
+        self.get_logger().info(
+            f'Published synthetic GPS origin '
+            f'(lat={self._lat}, lon={self._lon}, alt={self._alt}) '
+            f'[{self._publish_count}/3]'
+        )
+        # Publish a few times in case MAVROS subscribed late, then stop.
+        if self._publish_count >= 3:
+            self._done = True
+            self._timer.cancel()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    try:
+        node = MavrosGpOriginNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

From 71e92f268c05a3647a7c1db6040136f1ea30842a Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Fri, 12 Jun 2026 00:49:43 -0400
Subject: [PATCH 21/27] references pegasus sim drone body which is what moves.

---
 .../launch_scripts/example_multi_px4_pegasus_launch_script.py   | 2 +-
 .../launch_scripts/example_one_px4_pegasus_launch_script.py     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
index f6d0f2de1..72f81eb99 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
@@ -206,7 +206,7 @@ def _body_name(i: int) -> str:
                 return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{i}"
 
             drones = [
-                (_body_name(i), i, f"/World/drone{i}/base_link")
+                (_body_name(i), i, f"/World/drone{i}/base_link/body")
                 for i in range(1, NUM_ROBOTS + 1)
             ]
             self.natnet_manager = start_drone_natnet_server(stage, drones)
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
index d173b551f..8b9861307 100755
--- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
@@ -269,7 +269,7 @@ def _setup_natnet(self, stage):
         try:
             from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
-            drones = [(NATNET_BODY_NAME, 1, "/World/base_link")]
+            drones = [(NATNET_BODY_NAME, 1, "/World/base_link/body")]
             self.natnet_manager = start_drone_natnet_server(stage, drones)
             carb.log_warn(
                 f"[natnet] Emulator started with 1 body ('{NATNET_BODY_NAME}' -> /World/base_link)."

From db673b6588080277f72a5d3eed79f99a78e49aab Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Fri, 12 Jun 2026 13:41:32 -0400
Subject: [PATCH 22/27] added realistic optitrack sensor noise

---
 .../optitrack/natnet/emulator/isaac/config.py | 24 +++++++++++++++
 .../optitrack/natnet/emulator/isaac/frames.py | 29 +++++++++++++++++--
 .../natnet/emulator/isaac/manager.py          | 15 +++++++++-
 .../natnet/emulator/isaac/scene_setup.py      |  9 ++++++
 .../natnet/emulator/isaac/ui_extension.py     |  3 ++
 .../natnet/emulator/isaac/usd_bindings.py     | 12 ++++++++
 .../test/test_frames.py                       | 28 ++++++++++++++++++
 .../test/test_interface_authoring.py          | 17 +++++++++++
 .../test/test_interface_config.py             | 22 ++++++++++++++
 .../test/test_scene_setup.py                  | 13 +++++++++
 ...example_multi_px4_pegasus_launch_script.py | 10 ++++++-
 .../example_one_px4_pegasus_launch_script.py  | 10 ++++++-
 12 files changed, 187 insertions(+), 5 deletions(-)

diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
index 462846525..b0c18b555 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
@@ -32,6 +32,10 @@
 ATTR_NATNET_VERSION = "natnet:natnetVersion"
 ATTR_UP_AXIS = "natnet:upAxis"
 
+ATTR_POSE_NOISE_ENABLED = "natnet:poseNoiseEnabled"
+ATTR_POSE_NOISE_STD_METERS = "natnet:poseNoiseStdMeters"
+ATTR_POSE_NOISE_ROTATION_DEG = "natnet:poseNoiseRotationDeg"
+
 BODY_PREFIX = "natnet:body:"
 BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName"
 BODY_FIELD_STREAMING_ID = "streamingId"
@@ -55,6 +59,9 @@
 DEFAULT_PUBLISH_RATE = 100.0
 DEFAULT_NATNET_VERSION = "4.4.0.0"
 DEFAULT_UP_AXIS = "Z"
+DEFAULT_POSE_NOISE_ENABLED = True
+DEFAULT_POSE_NOISE_STD_METERS = 0.0005
+DEFAULT_POSE_NOISE_ROTATION_DEG = 0.05
 
 
 def body_attr_name(key: str, field_name: str) -> str:
@@ -147,6 +154,9 @@ class NatNetInterfaceConfig:
     publish_rate: float = DEFAULT_PUBLISH_RATE
     natnet_version: str = DEFAULT_NATNET_VERSION
     up_axis: str = DEFAULT_UP_AXIS
+    pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED
+    pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS
+    pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG
     bodies: list[BodyBinding] = field(default_factory=list)
 
     @classmethod
@@ -162,6 +172,9 @@ def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig":
             publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)),
             natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)),
             up_axis=str(d.get("up_axis", DEFAULT_UP_AXIS)).upper(),
+            pose_noise_enabled=bool(d.get("pose_noise_enabled", DEFAULT_POSE_NOISE_ENABLED)),
+            pose_noise_std_meters=float(d.get("pose_noise_std_meters", DEFAULT_POSE_NOISE_STD_METERS)),
+            pose_noise_rotation_deg=float(d.get("pose_noise_rotation_deg", DEFAULT_POSE_NOISE_ROTATION_DEG)),
             bodies=_normalize_bodies(d.get("bodies")),
         )
 
@@ -176,6 +189,9 @@ def to_dict(self) -> dict[str, Any]:
             "publish_rate": self.publish_rate,
             "natnet_version": self.natnet_version,
             "up_axis": self.up_axis,
+            "pose_noise_enabled": self.pose_noise_enabled,
+            "pose_noise_std_meters": self.pose_noise_std_meters,
+            "pose_noise_rotation_deg": self.pose_noise_rotation_deg,
             "bodies": [b.to_dict() for b in self.bodies],
         }
 
@@ -193,6 +209,14 @@ def validate(self) -> "NatNetInterfaceConfig":
             errors.append("command_port and data_port must differ")
         if self.publish_rate <= 0:
             errors.append(f"publish_rate must be > 0, got {self.publish_rate}")
+        if self.pose_noise_std_meters < 0:
+            errors.append(
+                f"pose_noise_std_meters must be >= 0, got {self.pose_noise_std_meters}"
+            )
+        if self.pose_noise_rotation_deg < 0:
+            errors.append(
+                f"pose_noise_rotation_deg must be >= 0, got {self.pose_noise_rotation_deg}"
+            )
         for i, body in enumerate(self.bodies):
             if not body.rigid_body_name:
                 errors.append(f"body[{i}] rigid_body_name must be non-empty")
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
index c767733b8..cadcc95be 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
@@ -22,8 +22,9 @@
 from __future__ import annotations
 
 import math
+import numpy as np
 from dataclasses import dataclass
-
+from scipy.spatial.transform import Rotation
 from ..server.natnet_data_types import sFrameOfMocapData, sRigidBodyData
 
 TRACKING_VALID = 0x01
@@ -46,7 +47,7 @@ def lost(cls, streaming_id: int) -> "BodySample":
         return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False)
 
 
-def to_motive_pose(position, orientation, up_axis: str = "Z"):
+def to_motive_pose(position: tuple[float, float, float], orientation: tuple[float, float, float, float], up_axis: str = "Z"):
     """Re-express an Isaac (Z-up) world pose in Motive's streamed up-axis frame.
 
     Returns ``(position, orientation)`` re-axed for the given ``up_axis``:
@@ -104,3 +105,27 @@ def build_frame(
 def is_finite_pose(sample: BodySample) -> bool:
     """True if all position/orientation components are finite (no NaN/inf)."""
     return all(math.isfinite(v) for v in (*sample.position, *sample.orientation))
+
+
+def apply_pose_noise(
+    position: tuple[float, float, float],
+    orientation: tuple[float, float, float, float],
+    pose_noise_std_meters: float,
+    pose_noise_rotation_deg: float,
+) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]:
+    """Add independent Gaussian noise to position (m) and orientation (deg, XYZ euler)."""
+
+    x, y, z = position
+    if pose_noise_std_meters > 0.0:
+        x += np.random.normal(0, pose_noise_std_meters)
+        y += np.random.normal(0, pose_noise_std_meters)
+        z += np.random.normal(0, pose_noise_std_meters)
+
+    roll, pitch, yaw = Rotation.from_quat(orientation).as_euler("xyz", degrees=True)
+    if pose_noise_rotation_deg > 0.0:
+        roll += np.random.normal(0, pose_noise_rotation_deg)
+        pitch += np.random.normal(0, pose_noise_rotation_deg)
+        yaw += np.random.normal(0, pose_noise_rotation_deg)
+
+    qx, qy, qz, qw = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=True).as_quat()
+    return (x, y, z), (float(qx), float(qy), float(qz), float(qw))
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
index fb9699cb1..09f7b0891 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -18,7 +18,7 @@
 
 from .catalog import build_catalog, find_duplicate_targets
 from .config import DEFAULT_UP_AXIS, NatNetInterfaceConfig
-from .frames import BodySample, build_frame, to_motive_pose
+from .frames import BodySample, apply_pose_noise, build_frame, to_motive_pose
 from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets
 
 
@@ -68,6 +68,10 @@ def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str:
     lines.append(f"  publishRate   : {cfg.publish_rate}")
     lines.append(f"  natnetVersion : {cfg.natnet_version}")
     lines.append(f"  upAxis        : {cfg.up_axis}")
+    lines.append(f"  poseNoise     : enabled={cfg.pose_noise_enabled}")
+    lines.append(
+        f"                  std={cfg.pose_noise_std_meters} m, rot={cfg.pose_noise_rotation_deg} deg"
+    )
     if cfg.bodies:
         lines.append(f"  bodies ({len(cfg.bodies)}):")
         for b in cfg.bodies:
@@ -103,6 +107,10 @@ def __init__(self, server_factory=None):
         # "Z" (default) streams the Isaac/USD world pose as-is; "Y" re-axes it to
         # emulate a default Y-up Motive. See frames.to_motive_pose.
         self._up_axis = DEFAULT_UP_AXIS
+        # Pose noise.
+        self._pose_noise_enabled = False
+        self._pose_noise_std_meters = 0.0
+        self._pose_noise_rotation_deg = 0.0
 
     # --- lifecycle -------------------------------------------------------------
 
@@ -283,6 +291,9 @@ def _resync(self, stage) -> bool:
             return False
         config = read_interface(interfaces[0])
         self._up_axis = config.up_axis
+        self._pose_noise_enabled = config.pose_noise_enabled
+        self._pose_noise_std_meters = config.pose_noise_std_meters
+        self._pose_noise_rotation_deg = config.pose_noise_rotation_deg
         if self._server is not None:
             self._server.set_model_def_payload(build_catalog(config).pack())
         # Cache target *paths* (not prim handles): the prim is re-resolved every
@@ -324,6 +335,8 @@ def sample_once(self, stage=None):
                 samples.append(BodySample.lost(streaming_id))
             else:
                 position, orientation = to_motive_pose(*pose, up_axis=self._up_axis)
+                if self._pose_noise_enabled:
+                    position, orientation = apply_pose_noise(position, orientation, self._pose_noise_std_meters, self._pose_noise_rotation_deg)
                 samples.append(BodySample(streaming_id, position, orientation, valid=True))
 
         frame = build_frame(
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
index 1844595dd..524c7f9b9 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
@@ -20,6 +20,9 @@
 from .config import (
     DEFAULT_COMMAND_PORT,
     DEFAULT_DATA_PORT,
+    DEFAULT_POSE_NOISE_ENABLED,
+    DEFAULT_POSE_NOISE_ROTATION_DEG,
+    DEFAULT_POSE_NOISE_STD_METERS,
     DEFAULT_PUBLISH_RATE,
     DEFAULT_SERVER_IP,
     DEFAULT_UP_AXIS,
@@ -44,6 +47,9 @@ def build_drone_config(
     publish_rate: float = DEFAULT_PUBLISH_RATE,
     server_enabled: bool = True,
     up_axis: str = DEFAULT_UP_AXIS,
+    pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED,
+    pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS,
+    pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG,
 ) -> NatNetInterfaceConfig:
     """Build a validated config with one rigid body per drone.
 
@@ -68,6 +74,9 @@ def build_drone_config(
         data_port=data_port,
         publish_rate=publish_rate,
         up_axis=up_axis,
+        pose_noise_enabled=pose_noise_enabled,
+        pose_noise_std_meters=pose_noise_std_meters,
+        pose_noise_rotation_deg=pose_noise_rotation_deg,
         bodies=bodies,
     )
     cfg.validate()
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index 656d8b88e..e83d742b6 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -192,6 +192,9 @@ def _build_window(self):
 
                 ui.Separator(height=6)
                 self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled)
+                self._bool_row(ui, "Pose noise enabled", "pose_noise_enabled", self._cfg.pose_noise_enabled)
+                self._float_row(ui, "Pose noise std meters", "pose_noise_std_meters", self._cfg.pose_noise_std_meters)
+                self._float_row(ui, "Pose noise rotation deg", "pose_noise_rotation_deg", self._cfg.pose_noise_rotation_deg)
                 self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip)
                 self._combo_row(ui, "Mode", "mode", self._cfg.mode, VALID_MODES)
                 self._int_row(ui, "Command port", "command_port", self._cfg.command_port)
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
index 3fddc4f58..870534252 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -22,6 +22,9 @@
     ATTR_MODE,
     ATTR_MULTICAST_ADDR,
     ATTR_NATNET_VERSION,
+    ATTR_POSE_NOISE_ENABLED,
+    ATTR_POSE_NOISE_ROTATION_DEG,
+    ATTR_POSE_NOISE_STD_METERS,
     ATTR_PUBLISH_RATE,
     ATTR_SERVER_ENABLED,
     ATTR_SERVER_IP,
@@ -35,6 +38,9 @@
     DEFAULT_DATA_PORT,
     DEFAULT_MULTICAST_ADDR,
     DEFAULT_NATNET_VERSION,
+    DEFAULT_POSE_NOISE_ENABLED,
+    DEFAULT_POSE_NOISE_ROTATION_DEG,
+    DEFAULT_POSE_NOISE_STD_METERS,
     DEFAULT_PUBLISH_RATE,
     DEFAULT_SERVER_IP,
     DEFAULT_UP_AXIS,
@@ -72,6 +78,9 @@ def author_interface(stage, prim_path: str, config: Any) -> Any:
     _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate)
     _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version)
     _set(prim, ATTR_UP_AXIS, Sdf.ValueTypeNames.Token, cfg.up_axis)
+    _set(prim, ATTR_POSE_NOISE_ENABLED, Sdf.ValueTypeNames.Bool, cfg.pose_noise_enabled)
+    _set(prim, ATTR_POSE_NOISE_STD_METERS, Sdf.ValueTypeNames.Float, cfg.pose_noise_std_meters)
+    _set(prim, ATTR_POSE_NOISE_ROTATION_DEG, Sdf.ValueTypeNames.Float, cfg.pose_noise_rotation_deg)
 
     for key, body in cfg.assign_instance_keys():
         _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name)
@@ -98,6 +107,9 @@ def read_interface(prim) -> NatNetInterfaceConfig:
         publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)),
         natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)),
         up_axis=str(_get(prim, ATTR_UP_AXIS, DEFAULT_UP_AXIS)),
+        pose_noise_enabled=bool(_get(prim, ATTR_POSE_NOISE_ENABLED, DEFAULT_POSE_NOISE_ENABLED)),
+        pose_noise_std_meters=float(_get(prim, ATTR_POSE_NOISE_STD_METERS, DEFAULT_POSE_NOISE_STD_METERS)),
+        pose_noise_rotation_deg=float(_get(prim, ATTR_POSE_NOISE_ROTATION_DEG, DEFAULT_POSE_NOISE_ROTATION_DEG)),
         bodies=_read_bodies(prim),
     )
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
index e26165d99..10feb0c46 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py
@@ -13,6 +13,7 @@
     MODEL_LIST_CHANGED,
     TRACKING_VALID,
     BodySample,
+    apply_pose_noise,
     build_frame,
     make_rigid_body_data,
     to_motive_pose,
@@ -67,6 +68,33 @@ def test_build_frame_no_bodies():
     assert frame.params == 0
 
 
+def test_apply_pose_noise_zero_std_is_identity():
+    position = (1.0, 2.0, 3.0)
+    orientation = (0.0, 0.0, 0.0, 1.0)
+    pos_out, quat_out = apply_pose_noise(position, orientation, 0.0, 0.0)
+    assert pos_out == position
+    assert quat_out == pytest.approx(orientation)
+
+
+def test_apply_pose_noise_preserves_y_position():
+    np = pytest.importorskip("numpy")
+    np.random.seed(0)
+    position = (0.0, 1.5, 0.0)
+    orientation = (0.0, 0.0, 0.0, 1.0)
+    pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0)
+    # Before the euler-yaw shadowing bug, y collapsed to ~0 instead of staying near 1.5.
+    assert pos_out[1] == pytest.approx(1.5, abs=0.01)
+
+
+def test_apply_pose_noise_adds_position_jitter():
+    np = pytest.importorskip("numpy")
+    np.random.seed(1)
+    position = (0.0, 0.0, 0.0)
+    orientation = (0.0, 0.0, 0.0, 1.0)
+    pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0)
+    assert pos_out != position
+
+
 def test_build_frame_multiple_bodies_preserve_order():
     samples = [
         BodySample(1, (1.0, 0.0, 0.0)),
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
index 9546ad9dc..10f4c8b4a 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
@@ -96,6 +96,23 @@ def test_up_axis_authors_and_reads_back():
     assert read_interface(find_interfaces(stage)[0]).up_axis == "Y"
 
 
+def test_pose_noise_authors_and_reads_back():
+    stage = _new_stage()
+    cfg = NatNetInterfaceConfig.from_dict(
+        {
+            **_CONFIG,
+            "pose_noise_enabled": False,
+            "pose_noise_std_meters": 0.001,
+            "pose_noise_rotation_deg": 0.1,
+        }
+    )
+    author_interface(stage, "/World/NatNetInterface", cfg)
+    read = read_interface(find_interfaces(stage)[0])
+    assert read.pose_noise_enabled is False
+    assert read.pose_noise_std_meters == pytest.approx(0.001)
+    assert read.pose_noise_rotation_deg == pytest.approx(0.1)
+
+
 def test_empty_target_round_trips():
     # The UI's "Add body" can create a body with no target yet (set later in the
     # Property panel); it must author and read back cleanly with an empty target.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
index ddfcd5705..9247c5f12 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py
@@ -11,6 +11,9 @@
 import pytest
 
 from optitrack.natnet.emulator.isaac.config import (
+    DEFAULT_POSE_NOISE_ENABLED,
+    DEFAULT_POSE_NOISE_ROTATION_DEG,
+    DEFAULT_POSE_NOISE_STD_METERS,
     BodyBinding,
     NatNetInterfaceConfig,
     body_attr_name,
@@ -28,6 +31,9 @@ def test_defaults_match_server_expectations():
     assert cfg.command_port == 1510
     assert cfg.data_port == 1511
     assert cfg.up_axis == "Z"  # Isaac/USD native; matches the reference Motive setup
+    assert cfg.pose_noise_enabled is DEFAULT_POSE_NOISE_ENABLED
+    assert cfg.pose_noise_std_meters == DEFAULT_POSE_NOISE_STD_METERS
+    assert cfg.pose_noise_rotation_deg == DEFAULT_POSE_NOISE_ROTATION_DEG
     assert cfg.bodies == []
 
 
@@ -43,6 +49,20 @@ def test_up_axis_survives_round_trip():
     assert NatNetInterfaceConfig.from_dict(cfg.to_dict()).up_axis == "Y"
 
 
+def test_pose_noise_survives_round_trip():
+    cfg = NatNetInterfaceConfig.from_dict(
+        {
+            "pose_noise_enabled": False,
+            "pose_noise_std_meters": 0.001,
+            "pose_noise_rotation_deg": 0.1,
+        }
+    )
+    restored = NatNetInterfaceConfig.from_dict(cfg.to_dict())
+    assert restored.pose_noise_enabled is False
+    assert restored.pose_noise_std_meters == 0.001
+    assert restored.pose_noise_rotation_deg == 0.1
+
+
 def test_from_dict_with_bodies_as_list():
     cfg = NatNetInterfaceConfig.from_dict(
         {
@@ -126,6 +146,8 @@ def test_assign_instance_keys_are_unique():
         {"publish_rate": 0},
         {"up_axis": "X"},
         {"up_axis": "bogus"},
+        {"pose_noise_std_meters": -0.001},
+        {"pose_noise_rotation_deg": -0.1},
     ],
 )
 def test_validate_rejects_bad_server_config(overrides):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
index d58bd9a1a..3492ed1ed 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py
@@ -85,3 +85,16 @@ def test_duplicate_ids_raise():
         build_drone_config(
             [("DroneA", 1, "/World/a"), ("DroneB", 1, "/World/b")]
         )
+
+
+def test_pose_noise_params_are_forwarded():
+    cfg = build_drone_config(
+        [("Drone", 1, "/World/base_link")],
+        pose_noise_enabled=False,
+        pose_noise_std_meters=0.001,
+        pose_noise_rotation_deg=0.1,
+    )
+    assert cfg.pose_noise_enabled is False
+    assert cfg.pose_noise_std_meters == 0.001
+    assert cfg.pose_noise_rotation_deg == 0.1
+    cfg.validate()
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
index 72f81eb99..7baca6ee3 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
@@ -209,7 +209,15 @@ def _body_name(i: int) -> str:
                 (_body_name(i), i, f"/World/drone{i}/base_link/body")
                 for i in range(1, NUM_ROBOTS + 1)
             ]
-            self.natnet_manager = start_drone_natnet_server(stage, drones)
+            self.natnet_manager = start_drone_natnet_server(
+                stage,
+                drones,
+                **{
+                    "pose_noise_enabled": True,
+                    "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm
+                    "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg
+                }
+            )
             carb.log_warn(f"[natnet] Emulator started with {len(drones)} body(ies).")
         except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
             carb.log_error(f"[natnet] Failed to start emulator: {exc}")
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
index 8b9861307..fadbc0682 100755
--- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
@@ -270,7 +270,15 @@ def _setup_natnet(self, stage):
             from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
             drones = [(NATNET_BODY_NAME, 1, "/World/base_link/body")]
-            self.natnet_manager = start_drone_natnet_server(stage, drones)
+            self.natnet_manager = start_drone_natnet_server(
+                stage, 
+                drones,
+                **{
+                    "pose_noise_enabled": True,
+                    "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm
+                    "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg
+                }
+            )
             carb.log_warn(
                 f"[natnet] Emulator started with 1 body ('{NATNET_BODY_NAME}' -> /World/base_link)."
             )

From 24f410ff435e6c17fc59475d9cf3c40507838c6f Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Fri, 12 Jun 2026 13:41:58 -0400
Subject: [PATCH 23/27] adjusted position and orientation covariances to match
 optitrack's advertised tolerances

---
 .../perception/natnet_ros2/config/natnet_config.yaml | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
index dc0b3a0e0..a0e3882ee 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
+++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
@@ -41,11 +41,11 @@
     debug: false
 
     position_covariance:
-      [0.1, 0.0, 0.0,
-       0.0, 0.1, 0.0,
-       0.0, 0.0, 0.1]
+      [1.0e-6, 0.0, 0.0,
+       0.0, 1.0e-6, 0.0,
+       0.0, 0.0, 1.0e-6]
 
     orientation_covariance:
-      [0.01, 0.0, 0.0,
-       0.0, 0.01, 0.0,
-       0.0, 0.0, 0.01]
+      [3.0e-6, 0.0, 0.0,
+       0.0, 3.0e-6, 0.0,
+       0.0, 0.0, 3.0e-6]

From 5883321c160f2edb5892116b5083fae5183ab96e Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Mon, 15 Jun 2026 17:15:57 -0400
Subject: [PATCH 24/27] fixed commentsand some docs

---
 gcs/docker/gcs-base-docker-compose.yaml             |  2 --
 overrides/isaac-natnet-vision.env                   |  2 --
 .../src/perception/natnet_ros2/CMakeLists.txt       |  5 ++---
 robot/ros_ws/src/perception/natnet_ros2/README.md   |  7 ++-----
 .../natnet_ros2/config/natnet_config.yaml           |  4 ++--
 .../natnet_ros2/launch/mavros_gp_origin.launch.xml  |  2 +-
 .../launch/vision_pose_converter.launch.xml         |  2 +-
 .../perception/natnet_ros2/src/natnet_ros2_node.cpp | 13 +++----------
 .../isaac-sim/docker/px4-profiles/default.env       |  4 +---
 9 files changed, 12 insertions(+), 29 deletions(-)

diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 4fe87155a..c8ac5200e 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -36,8 +36,6 @@ services:
       - NVIDIA_DRIVER_CAPABILITIES=all
       # Number of robots (used by action_relay to spawn per-robot relays)
       - NUM_ROBOTS=${NUM_ROBOTS:-1}
-      # Launching NatNet for Optitrack Motion Capture
-      - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
       # Record bags
       - RECORD_BAGS=${RECORD_BAGS}
     image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env
index d085175c9..2cebb6468 100644
--- a/overrides/isaac-natnet-vision.env
+++ b/overrides/isaac-natnet-vision.env
@@ -1,7 +1,5 @@
 # Isaac Sim + NatNet mocap with PX4 external-vision fusion.
 # Usage: airstack up --env-file overrides/isaac-natnet-vision.env
-#
-# Root .env is always loaded first; this file overrides selected keys.
 
 LAUNCH_NATNET=true
 PX4_PARAM_PROFILE=vision
diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
index 4b72ed3d2..86342cb7c 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
+++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt
@@ -48,8 +48,7 @@ if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}")
 
   # Install libNatNet.so alongside the node and register an environment hook so
   # that sourcing the workspace adds lib/natnet_ros2/ to LD_LIBRARY_PATH.
-  # Use PROGRAMS (not FILES) to preserve the execute bit — shared libraries
-  # must be executable for the dynamic linker to map them.
+  # Use PROGRAMS (not FILES) to preserve the execute bit
   install(PROGRAMS "${_NATNET_LIB}"
     DESTINATION lib/${PROJECT_NAME})
 
@@ -65,7 +64,7 @@ else()
 endif()
 
 # ---------------------------------------------------------------------------
-# Python nodes (vision_pose_converter remains Python)
+# Python nodes
 # ---------------------------------------------------------------------------
 install(PROGRAMS
   src/vision_pose_converter_node.py
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index 2fb5b69cd..2a36a6280 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -30,7 +30,7 @@ NatNet ROS 2 Node
     ├→ /robot_1/perception/optitrack/{body_name}/pose_cov  (PoseWithCovarianceStamped, always)
     └→ (Optional, publish_to_mavros: true)
          mavros_gp_origin_node
-           └→ /robot_1/interface/mavros/global_position/set_gp_origin  (once, guarded)
+           └→ /robot_1/interface/mavros/global_position/set_gp_origin
          vision_pose_converter_node
            ├→ /robot_1/interface/mavros/vision_pose/pose
            └→ /robot_1/interface/mavros/vision_pose/pose_cov
@@ -71,10 +71,7 @@ When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose
 
 ##### Synthetic GPS origin (mocap / no-GNSS arming)
 
-With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fuses vision into a valid *local*
-position but has **no global position**. Modes that require one — such as
-`AUTO.LOITER` — then fail preflight and refuse to arm (`Arming denied: Resolve
-system health failures first`). When `publish_to_mavros: true`,
+With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `publish_to_mavros: true`,
 `mavros_gp_origin_node` publishes a synthetic origin once at startup:
 
 - **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped`
diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
index a0e3882ee..1e0ad6916 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
+++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
@@ -41,11 +41,11 @@
     debug: false
 
     position_covariance:
-      [1.0e-6, 0.0, 0.0,
+      [1.0e-6, 0.0, 0.0, # Covariances for sub 0.1 mm precision from optitrack
        0.0, 1.0e-6, 0.0,
        0.0, 0.0, 1.0e-6]
 
     orientation_covariance:
-      [3.0e-6, 0.0, 0.0,
+      [3.0e-6, 0.0, 0.0, # Covariances for sub 0.1 degree precision from optitrack
        0.0, 3.0e-6, 0.0,
        0.0, 0.0, 3.0e-6]
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
index 4013b7dda..4cb1f785b 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml
@@ -6,7 +6,7 @@
     Publishes a synthetic GPS origin once at startup for mocap / no-GNSS flight.
     Included from natnet_ros2.launch.py when publish_to_mavros is true.
 
-    Output (consumed by MAVROS under interface bringup namespace):
+    Output (consumed by MAVROS):
       /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin
   -->
 
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
index cc15983fd..209b22059 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
@@ -12,7 +12,7 @@
     Input  (from NatNet node):
       /{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov
 
-    Output (consumed by MAVROS under interface bringup namespace):
+    Output (consumed by MAVROS):
       /{ROBOT_NAME}/interface/mavros/vision_pose/pose
       /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov
   -->
diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
index 7e6044ac3..c93890e0d 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
+++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
@@ -119,11 +119,7 @@ class NatNetROS2Node : public rclcpp::Node
         client_ = std::make_unique<natnet_ros2::NatNetClientAdapter>();
         connect_cfg_ = connect_cfg;
 
-        // Try to connect now; if the server is not up yet keep retrying. The
-        // NatNet server may legitimately start *after* the robot — e.g. the Isaac
-        // Sim emulator only binds ~100 s into sim boot, and a real Motive PC may be
-        // powered on after the drone. A one-shot connect would leave us dead for
-        // the whole session, so retry on a timer until the first handshake lands.
+        // Try to connect now; keep retrying.
         if (!connect_and_setup(connect_cfg_)) {
             connect_timer_ = this->create_wall_timer(
                 std::chrono::seconds(2),
@@ -211,8 +207,7 @@ class NatNetROS2Node : public rclcpp::Node
 
 private:
     // -----------------------------------------------------------------------
-    // Returns true once the handshake succeeds and the frame callback is live.
-    // On failure it logs (WARN) and returns false so the caller can retry.
+    // Returns true once the handshake succeeds.
     bool connect_and_setup(const natnet_ros2::ConnectConfig & cfg)
     {
         const natnet_ros2::NegotiationResult neg =
@@ -239,9 +234,7 @@ class NatNetROS2Node : public rclcpp::Node
     }
 
     // -----------------------------------------------------------------------
-    // Timer-driven reconnect: fires every 2 s until the first handshake lands,
-    // then cancels itself. Runs on the node's executor thread (same as the
-    // refresh timer), so no extra locking versus single-threaded init.
+    // Timer-driven reconnect.
     void retry_connect()
     {
         if (connected_) {
diff --git a/simulation/isaac-sim/docker/px4-profiles/default.env b/simulation/isaac-sim/docker/px4-profiles/default.env
index b9b3f54bc..7d00f82cf 100644
--- a/simulation/isaac-sim/docker/px4-profiles/default.env
+++ b/simulation/isaac-sim/docker/px4-profiles/default.env
@@ -2,6 +2,4 @@
 #
 # Selected by PX4_PARAM_PROFILE in the top-level .env (compose interpolation).
 # Variables here are injected into the isaac-sim container and applied at PX4 boot
-# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS.
-#
-# See vision.env for external-vision / NatNet → MAVROS fusion tuning.
+# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS.
\ No newline at end of file

From 8279d900cd811e4679243e4f36cf866cf8caa6dd Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Tue, 16 Jun 2026 14:36:52 -0400
Subject: [PATCH 25/27] removed EV delay from sim

---
 simulation/isaac-sim/docker/px4-profiles/vision.env | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/isaac-sim/docker/px4-profiles/vision.env b/simulation/isaac-sim/docker/px4-profiles/vision.env
index 83dacd113..3a38ed65e 100644
--- a/simulation/isaac-sim/docker/px4-profiles/vision.env
+++ b/simulation/isaac-sim/docker/px4-profiles/vision.env
@@ -21,7 +21,7 @@ PX4_PARAM_EKF2_GPS_CTRL=0
 PX4_PARAM_EKF2_EV_NOISE_MD=0
 
 # End-to-end delay: NatNet frame + ROS + MAVROS (milliseconds). Tune if fusion lags.
-PX4_PARAM_EKF2_EV_DELAY=20
+# PX4_PARAM_EKF2_EV_DELAY=20
 
 # Mocap tracks base_link at the vehicle CG — no lever arm.
 PX4_PARAM_EKF2_EV_POS_X=0

From 83b5929dd306c86e91a2a762a6e634a3c54a5d7b Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Tue, 16 Jun 2026 19:32:39 -0400
Subject: [PATCH 26/27] mass code cleaning

---
 .agents/skills/add-unit-tests/SKILL.md        |  78 ++++++---
 .agents/skills/optitrack-development/SKILL.md |   4 +-
 .agents/skills/run-system-tests/SKILL.md      |   2 +-
 .env                                          |   5 -
 AGENTS.md                                     |   6 +-
 .../development/intermediate/testing/index.md |   4 +-
 .../intermediate/testing/unit_testing.md      |   2 +-
 docs/simulation/isaac_sim/docker.md           |   6 +
 .../isaac_sim/pegasus_scene_setup.md          |  14 +-
 overrides/isaac-natnet-vision.env             |   2 +-
 .../src/perception/natnet_ros2/README.md      |   2 +-
 .../natnet_ros2/test/test_natnet_ros2.py      |  15 +-
 .../isaac-sim/docker/docker-compose.yaml      |   5 +-
 .../isaac-sim/docker/px4-profiles/default.env |   5 -
 .../isaac-sim/docker/sitl-files/default.env   |   7 +
 .../vision.env => sitl-files/px4-vision.env}  |   4 +-
 .../optitrack.natnet.emulator/.gitignore      |   7 -
 .../optitrack.natnet.emulator/README.md       | 147 +++++++++++++----
 .../docs/natnet_interface_prim_design.md      |   4 +-
 .../optitrack/natnet/emulator/defaults.py     |   5 +-
 .../natnet/emulator/isaac/__init__.py         |   5 +-
 .../natnet/emulator/isaac/catalog.py          |   3 +-
 .../optitrack/natnet/emulator/isaac/config.py |  16 +-
 .../optitrack/natnet/emulator/isaac/frames.py |  20 ++-
 .../natnet/emulator/isaac/manager.py          |  41 ++---
 .../natnet/emulator/isaac/scene_setup.py      |  11 +-
 .../natnet/emulator/isaac/ui_extension.py     |  22 +--
 .../natnet/emulator/isaac/usd_bindings.py     |  16 +-
 .../emulator/server/natnet_data_types.py      |  14 +-
 .../emulator/server/natnet_model_types.py     |   8 +-
 .../natnet/emulator/server/natnet_server.py   |  35 ++--
 .../emulator/server/natnet_unicast_server.py  |  23 ++-
 .../test/conftest.py                          |   9 +-
 .../test/test_catalog.py                      |   6 +-
 .../test/test_discovery.py                    |   2 +-
 .../test/test_interface_authoring.py          |   7 +-
 .../test/test_pose_sampling.py                |   7 +-
 .../test/test_pose_streaming.py               |   7 +-
 .../test/test_server_from_config.py           |   8 +-
 .../test/test_server_lifecycle.py             |   8 +-
 .../test/test_target_resolution.py            |   8 +-
 .../test/test_unicast_protocol.py             |   2 +-
 ...example_multi_px4_pegasus_launch_script.py |   9 +-
 .../example_one_px4_pegasus_launch_script.py  |   9 +-
 tests/README.md                               |   7 +-
 tests/conftest.py                             |  60 ++++---
 tests/docker/docker-compose.yaml              |   3 -
 tests/integration/README.md                   |  21 +--
 tests/integration/natnet/README.md            | 149 ++++++++++++------
 .../natnet/test_natnet_integration.py         |  38 ++---
 tests/pytest.ini                              |   3 +-
 tests/robot/README.md                         |  10 +-
 .../natnet_ros2/test_natnet_ros2.py           |  15 +-
 .../test_validation_core.py                   |   6 +-
 .../optitrack_natnet_emulator/test_catalog.py |   6 +-
 .../test_defaults_and_catalog.py              |   6 +-
 .../test_discovery.py                         |   6 +-
 .../optitrack_natnet_emulator/test_frames.py  |   6 +-
 .../test_interface_authoring.py               |  10 +-
 .../test_interface_config.py                  |   6 +-
 .../test_pose_sampling.py                     |   6 +-
 .../test_pose_streaming.py                    |   6 +-
 .../test_scene_setup.py                       |   6 +-
 .../test_serializers.py                       |   6 +-
 .../test_server_from_config.py                |   6 +-
 .../test_server_lifecycle.py                  |   6 +-
 .../test_target_resolution.py                 |   6 +-
 .../test_unicast_protocol.py                  |   6 +-
 tests/system/test_liveliness.py               |  23 +--
 69 files changed, 533 insertions(+), 510 deletions(-)
 delete mode 100644 simulation/isaac-sim/docker/px4-profiles/default.env
 create mode 100644 simulation/isaac-sim/docker/sitl-files/default.env
 rename simulation/isaac-sim/docker/{px4-profiles/vision.env => sitl-files/px4-vision.env} (88%)

diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md
index 2ab4234fe..ea47f7b75 100644
--- a/.agents/skills/add-unit-tests/SKILL.md
+++ b/.agents/skills/add-unit-tests/SKILL.md
@@ -35,11 +35,12 @@ robot/ros_ws/src/<layer>/<package>/
 └── CMakeLists.txt              # wires ament_add_gtest under BUILD_TESTING
 
 tests/robot/<layer>/<package>/
-└── test_<name>.py              # ← thin PROXY (re-exports tests from above)
+└── test_<name>.py              # ← thin PROXY (registers tests from above)
 ```
 
-The **proxy** is a one-file shim that loads the real test module with `importlib`
-and re-exports every `test_*` function. This means:
+The **proxy** is a one-file shim that calls ``register_unit_tests()`` to load the
+real test module with ``importlib`` and expose every ``test_*`` function to pytest.
+This means:
 
 | Invocation | What runs |
 |---|---|
@@ -48,6 +49,39 @@ and re-exports every `test_*` function. This means:
 | CI `system-tests.yml` (PR open / approved) | Same path via `pytest tests/` |
 | `colcon test --packages-select <pkg>` | Real test in `package/test/` directly |
 
+### `register_unit_tests` (in `tests/conftest.py`)
+
+Proxies call this helper; they do not duplicate test logic. Signature:
+
+```python
+register_unit_tests(target_globals, test_dir, *module_files)
+```
+
+| Argument | Meaning |
+|---|---|
+| `target_globals` | Pass ``globals()`` from the proxy module — pytest collects ``test_*`` names injected here |
+| `test_dir` | Co-located test directory, usually ``repo_path("robot/ros_ws/src/<layer>/<pkg>/test")`` |
+| `*module_files` | One or more filenames under ``test_dir`` (e.g. ``"test_foo.py"``); fold several into one proxy |
+
+**What it does (in order):**
+
+1. Prepends ``test_dir`` and ``test_dir.parent`` (package or extension root) to
+   ``sys.path`` so loaded modules can import production code and sibling helpers.
+2. Loads each file via ``importlib.util.spec_from_file_location`` under a
+   synthetic name (``_unit_<parent>_<stem>``) so proxy and source can share the
+   same basename without circular imports.
+3. Copies every ``test_*`` callable from the loaded module into ``target_globals``.
+4. Wraps each with ``pytest.mark.unit`` so ``pytest tests/ -m unit`` selects them
+   even if the source omitted ``pytestmark``.
+
+**What it does not do:** run tests, install packages, or replace ``colcon test``.
+For local iteration against source only, run ``pytest <package>/test/`` (add a tiny
+``conftest.py`` in that dir for ``sys.path`` — see the emulator example below).
+
+Pair with ``repo_path()`` from the same conftest — paths are anchored on
+``AIRSTACK_ROOT`` (CI export, repo root locally). **Never** use
+``Path(__file__).parents[N]`` in a proxy.
+
 ## Step-by-Step: Adding a Python Unit Test
 
 ### 1. Identify pure-Python logic to test
@@ -114,39 +148,29 @@ For `rclpy.node.Node` subclasses use a real dummy base class instead of a
 
 ### 3. Write the thin proxy in tests/robot/
 
-Create `tests/robot/<layer>/<package>/test_<name>.py`. Use the shared
-`reexport_unit_tests` + `repo_path` helpers from `tests/conftest.py` so the proxy
-stays a two-call shim and the cross-tree path is anchored on `AIRSTACK_ROOT`
-(exported by CI, defaults to the repo root locally) — **never** count
-`Path(__file__).parents[N]` or hardcode `sys.path` walks in the proxy:
+Create `tests/robot/<layer>/<package>/test_<name>.py`. Use
+``register_unit_tests`` + ``repo_path`` from ``tests/conftest.py`` so the proxy
+stays a two-call shim:
 
 ```python
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes <package> unit tests from the package source tree.
+"""Proxy: registers <package> unit tests from the package source tree.
 
-Unit test logic lives co-located with the package source (ROS 2 / colcon convention):
+Unit test logic lives co-located with the package (ROS 2 / colcon convention):
   robot/ros_ws/src/<layer>/<package>/test/test_<name>.py
 
-This file makes those tests discoverable by ``pytest tests/`` (CI) and
-``airstack test -m unit`` without any changes to the CI workflow.
+Discoverable by ``pytest tests/`` (CI) and ``airstack test -m unit``.
 """
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("robot/ros_ws/src/<layer>/<package>/test"),
     "test_<name>.py",   # pass several filenames to fold multiple modules into one proxy
 )
 ```
 
-`reexport_unit_tests` (in `tests/conftest.py`) execs each co-located module with
-`importlib` under a unique name (avoiding the same-filename circular import), puts
-both the test dir and its parent (the package root) on `sys.path` so the source
-can import its package and sibling helpers, and tags every re-exported `test_*`
-with `pytest.mark.unit`. Because the root `conftest` is imported before any proxy
-is collected, `from conftest import ...` resolves in both CI and local runs.
-
 For a **direct** `pytest <package>/test/` (or `colcon test`) run — which bypasses
 the proxies — add a tiny `conftest.py` in the package `test/` dir that puts the
 package/extension root on `sys.path` (see
@@ -251,16 +275,16 @@ there are listed in [`tests/colcon_unit_test_packages.yaml`](../../../tests/colc
 
 The same proxy pattern applies verbatim:
 
-**Sim-side Python** (e.g. motive emulator protocol logic):
+**Sim-side Python** (e.g. OptiTrack NatNet emulator):
 ```
 simulation/.../<tool>/test/test_<name>.py   ← source
-tests/sim/<tool>/test_<name>.py             ← proxy (parents[3] = repo root)
+tests/sim/<tool>/test_<name>.py             ← proxy (register_unit_tests + repo_path)
 ```
 
 **GCS modules**:
 ```
 gcs/.../<pkg>/test/test_<name>.py           ← source
-tests/gcs/<pkg>/test_<name>.py             ← proxy (parents[3] = repo root)
+tests/gcs/<pkg>/test_<name>.py             ← proxy (register_unit_tests + repo_path)
 ```
 
 `pytest tests/ -m unit` discovers them through the proxy without any
@@ -274,7 +298,8 @@ pytest.ini or CI changes needed.
 |---|---|
 | Where does test source live? | `<component>/…/<package>/test/` (co-located with the package) |
 | Where does pytest discover tests? | `tests/robot/` (or `tests/sim/`, `tests/gcs/`) via thin proxy |
-| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique module name |
+| How does the proxy register tests? | ``register_unit_tests(globals(), repo_path(...), "test_*.py")`` in `tests/conftest.py` |
+| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique synthetic module name |
 | What mark do all unit tests use? | `@pytest.mark.unit` |
 | What CI workflow runs them? | `system-tests.yml` — runs `pytest tests/` which includes unit tests |
 | When does that workflow trigger? | PR opened, `/pytest` comment, `workflow_dispatch` |
@@ -296,8 +321,9 @@ Corresponding proxies: `tests/robot/perception/natnet_ros2/test_natnet_ros2.py`,
 ## Files to Know
 
 - `.github/workflows/system-tests.yml` — CI workflow (runs `pytest tests/` including unit tests)
+- `tests/conftest.py` — `register_unit_tests`, `repo_path`, shared fixtures
 - `tests/pytest.ini` — mark registration (`unit`, `build_docker`, etc.)
 - `tests/robot/` — proxy layer mirroring `robot/ros_ws/src/`
-- `tests/sim/` — proxy layer for sim-side code (future)
+- `tests/sim/` — proxy layer for sim-side extensions and tools
 - `tests/gcs/` — proxy layer for GCS code (future)
 - `tests/README.md` — full test harness reference
diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md
index 4120510c4..c13aba904 100644
--- a/.agents/skills/optitrack-development/SKILL.md
+++ b/.agents/skills/optitrack-development/SKILL.md
@@ -39,7 +39,7 @@ flowchart LR
 | Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) |
 | SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) |
 | Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests |
-| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (marks: `integration`, `natnet`) |
+| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (mark: `integration`) |
 
 **Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`.
 
@@ -198,7 +198,7 @@ Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not
 |-------|----------|-----------|
 | Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names |
 | Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model |
-| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (marks: `integration`, `natnet`) |
+| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (mark: `integration`) |
 | System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` |
 
 ```bash
diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md
index 453bbc953..6cf7fbaa7 100644
--- a/.agents/skills/run-system-tests/SKILL.md
+++ b/.agents/skills/run-system-tests/SKILL.md
@@ -25,7 +25,7 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the
 The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`.
 
 - **`tests/system/`** — Docker stack integration tests. Marks: `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`.
-- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that re-export tests from each ROS 2 package's own `test/` directory (co-located with the source, the ROS 2 / colcon convention). The proxy pattern keeps test source next to the code it tests while making tests discoverable by `pytest tests/`.
+- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that call ``register_unit_tests()`` to register tests from each package's co-located `test/` directory so `pytest tests/` discovers them.
 
 ### Unit tests vs system tests
 
diff --git a/.env b/.env
index 2633b1f8e..0d994bc51 100644
--- a/.env
+++ b/.env
@@ -51,11 +51,6 @@ URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
 
 DEBUG_RVIZ="false"  # "true" or "false". If true, launches RViz alongside the robot via desktop_bringup/robot.launch.xml.
 
-LAUNCH_NATNET="false"
-# PX4 SITL param profile for isaac-sim (simulation/isaac-sim/docker/px4-profiles/).
-# Use "vision" with LAUNCH_NATNET + MAVROS vision_pose; "default" for plain Pegasus SITL.
-PX4_PARAM_PROFILE="default"
-
 # offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication. 
 OFFBOARD_BASE_PORT=14540
 ONBOARD_BASE_PORT=14580
diff --git a/AGENTS.md b/AGENTS.md
index fa00915a4..dbb70f09a 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -197,15 +197,15 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo <topic_name> --onc
    - Verify module behavior in isolation
    - Test with synthetic data
    - Located in module's `test/` directory
-   - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m <mark>` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`unit`, `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`, `integration`, `natnet`).
+   - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m <mark>` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`unit`, `build_docker`, `build_packages`, `integration`, `liveliness`, `sensors`, `takeoff_hover_land`).
 
    ```bash
    docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+"
    ```
 
-2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `<package>/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) re-export those tests so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill.
+2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `<package>/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) call `register_unit_tests()` so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill.
 
-3. **Integration (`pytest`, `integration` mark, [`tests/integration/`](tests/integration/)):** Wire a few real components together — the robot autonomy container plus a host-side component — **without** a sim or GPU. The shared `robot_autonomy_stack` fixture reuses a running `robot-desktop` container or brings one up only when `--run-integration` is passed (else skips), so a plain `pytest tests/` never spins up Docker. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz; marks `integration`, `natnet`). Example: `airstack test -m integration --run-integration -v`.
+3. **Integration (`pytest`, `integration` mark, [`tests/integration/`](tests/integration/)):** Wire a few real components together — the robot autonomy container plus a host-side component — **without** a sim or GPU. The shared `robot_autonomy_stack` fixture reuses a running `robot-desktop` container or brings one up only when `--run-integration` is passed (else skips), so a plain `pytest tests/` never spins up Docker. Collection order runs integration after `build_docker` and `build_packages`. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz). Example: `airstack test -m integration --run-integration -v`.
 
 4. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy)
    - End-to-end autonomy stack testing
diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md
index 0769e2b09..2a61b2e34 100644
--- a/docs/development/intermediate/testing/index.md
+++ b/docs/development/intermediate/testing/index.md
@@ -12,8 +12,8 @@ hardware requirement:
 ## Unit tests (`pytest -m unit`)
 
 Fast, hermetic Python tests that run in seconds with no Docker or GPU. Test source
-lives **co-located with its ROS 2 package** (`<package>/test/`) and is re-exported
-through thin proxy files in `tests/robot/` for centralized discovery.
+lives **co-located with its ROS 2 package** (`<package>/test/`) and is registered
+through thin proxy files in `tests/robot/` via `register_unit_tests()`.
 
 ```bash
 airstack test -m unit -v
diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md
index 2b97c787d..6d0a1b220 100644
--- a/docs/development/intermediate/testing/unit_testing.md
+++ b/docs/development/intermediate/testing/unit_testing.md
@@ -5,7 +5,7 @@ AirStack unit tests are **fast, hermetic, and purely Python** — no Docker stac
 ## Design principles
 
 - **Co-located with source.** Test files live in `<package>/test/` alongside the code they test. This is the standard ROS 2 / colcon convention and ensures tests are discovered by both `colcon test` and `pytest`.
-- **Proxy for centralized discovery.** A thin shim in `tests/robot/<layer>/<package>/` re-exports the test functions so `pytest tests/` (the CI command) and `airstack test -m unit` discover them without any changes to the CI workflow.
+- **Proxy for centralized discovery.** A thin shim in `tests/robot/<layer>/<package>/` calls `register_unit_tests()` (in `tests/conftest.py`) to register co-located `test_*` functions so `pytest tests/` and `airstack test -m unit` discover them. See the `add-unit-tests` skill for the full contract.
 - **`@pytest.mark.unit` on every test.** The `unit` mark is the filter that keeps unit tests isolated from system tests that need Docker, GPUs, and sim licenses.
 
 ## Repository layout
diff --git a/docs/simulation/isaac_sim/docker.md b/docs/simulation/isaac_sim/docker.md
index 74edf49f8..8956aab34 100644
--- a/docs/simulation/isaac_sim/docker.md
+++ b/docs/simulation/isaac_sim/docker.md
@@ -12,6 +12,9 @@ simulation/isaac-sim/docker/
 ├── fastdds.xml                   # DDS configuration
 ├── omni_pass.env                 # Omniverse credentials (git-ignored)
 ├── omni_pass_TEMPLATE.env        # Template for credentials
+├── sitl-files/                   # SITL env bundles (PX4_PARAM_* via compose)
+│   ├── default.env               # Plain Pegasus SITL (no param overrides)
+│   └── px4-vision.env            # External vision / NatNet → MAVROS → EKF2
 ├── omniverse.toml                # Omniverse settings
 ├── user.config.json              # Isaac Sim configuration (enables extensions)
 └── user_TEMPLATE.config.json     # Template configuration
@@ -116,11 +119,14 @@ Key variables for Isaac Sim configuration:
 | `ISAAC_SIM_SCRIPT_NAME` | Standalone script filename | - |
 | `PX4_PHYSICS_HZ` | Physics step rate for PX4 SITL — also sets PX4 `IMU_INTEG_RATE` | `250` |
 | `PX4_RENDERING_HZ` | Rendering frame rate for PX4 profiles (independent of physics) | `60` |
+| `SITL_PARAM_PROFILE` | SITL env bundle stem under `sitl-files/` (e.g. `default`, `px4-vision`) | `default` |
 | `ARDUPILOT_PHYSICS_HZ` | Physics step rate for ArduPilot SITL | `800` |
 | `ARDUPILOT_RENDERING_HZ` | Rendering frame rate for ArduPilot profiles | `120` |
 
 `PX4_PHYSICS_HZ` and `PX4_RENDERING_HZ` are set in the top-level `.env`. Pegasus defaults to 250 Hz but AirStack runs PX4 at **100 Hz** for near-real-time performance. See [Pegasus Scene Setup → Physics Rate](pegasus_scene_setup.md) for valid values and the full configuration flow.
 
+SITL parameter bundles live in `sitl-files/` (`default.env`, `px4-vision.env`). Compose loads `sitl-files/${SITL_PARAM_PROFILE}.env` into the container; see [Pegasus Scene Setup → PX4 SITL env bundles](pegasus_scene_setup.md#px4-sitl-env-bundles-external-vision).
+
 **Example overrides:**
 
 ```bash
diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md
index 72b3bdc97..5d1dc6f17 100644
--- a/docs/simulation/isaac_sim/pegasus_scene_setup.md
+++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md
@@ -165,19 +165,19 @@ PX4_RENDERING_HZ="30"
 - **`PX4_PHYSICS_HZ`** — Sets `physics_dt = 1 / PX4_PHYSICS_HZ` in Isaac Sim's physics scene, and automatically syncs PX4's `IMU_INTEG_RATE` parameter to the same value via `PX4LaunchTool` → `px4-rc.simulator`. Patched within the Docker image to read the environment variable and set the IMU_INTEG_RATE parameter.
 - **`PX4_RENDERING_HZ`** — Sets the rendering frame rate independently of physics. 30 Hz rendering has no effect on physics accuracy or PX4 behavior, but does slightly affect performance due to resource usage.
 
-### PX4 parameter profiles (external vision)
+### PX4 SITL env bundles (external vision)
 
-Isaac Sim loads a layered PX4 SITL parameter profile from `simulation/isaac-sim/docker/px4-profiles/`. Select the profile with `PX4_PARAM_PROFILE` in the top-level `.env`:
+Isaac Sim loads optional PX4 SITL parameter bundles from `simulation/isaac-sim/docker/sitl-files/`. Select the bundle with `SITL_PARAM_PROFILE` in the top-level `.env` (compose maps this to `sitl-files/<profile>.env`):
 
 ```bash
-# Plain Pegasus SITL
-PX4_PARAM_PROFILE="default"
+# Plain Pegasus SITL (default.env — comments only, no PX4_PARAM_* overrides)
+SITL_PARAM_PROFILE="default"
 
-# NatNet / mocap → MAVROS vision_pose → EKF2 external vision
-PX4_PARAM_PROFILE="vision"
+# NatNet / mocap → MAVROS vision_pose → EKF2 external vision (px4-vision.env)
+SITL_PARAM_PROFILE="px4-vision"
 ```
 
-Profiles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `vision` with robot-side `LAUNCH_NATNET=true` and `publish_to_mavros: true` in `natnet_config.yaml`.
+Bundles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `px4-vision` with robot-side `LAUNCH_NATNET=true` and `publish_to_mavros: true` in `natnet_config.yaml`.
 
 Convenience bundle: `airstack up --env-file overrides/isaac-natnet-vision.env`.
 
diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env
index 2cebb6468..097128c60 100644
--- a/overrides/isaac-natnet-vision.env
+++ b/overrides/isaac-natnet-vision.env
@@ -2,6 +2,6 @@
 # Usage: airstack up --env-file overrides/isaac-natnet-vision.env
 
 LAUNCH_NATNET=true
-PX4_PARAM_PROFILE=vision
+SITL_PARAM_PROFILE=px4-vision
 ISAAC_SIM_USE_STANDALONE=true
 ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index 2a36a6280..c357d86be 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -67,7 +67,7 @@ When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose
 - **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message)
 - **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized)
 - **Enabled by**: `publish_to_mavros: true` in config
-- **PX4 side**: set `PX4_PARAM_PROFILE=vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/px4-profiles/vision.env`
+- **PX4 side**: set `SITL_PARAM_PROFILE=px4-vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/sitl-files/px4-vision.env`
 
 ##### Synthetic GPS origin (mocap / no-GNSS arming)
 
diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
index cba5a3444..913b58161 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
+++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
@@ -1,18 +1,11 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Unit tests for natnet_ros2 Python source code.
+"""Unit tests for natnet_ros2 Python helpers (no ROS install required).
 
-These tests import the actual production source files and stub out ROS at the
-import boundary so no ROS installation is required.
+Stubs rclpy at import time. Covers ``VisionPoseConverterNode`` quaternion
+canonicalisation and frame_id assignment.
 
-Coverage here:
-  vision_pose_converter_node.py  → VisionPoseConverterNode._canonical_quaternion()
-                                 → VisionPoseConverterNode._on_pose() frame_id assignment
-
-NOT covered here (C++ — requires colcon build + gtest):
-  natnet_ros2_node.cpp           → build_covariance_6x6(), topic name construction,
-                                   connection_type validation, SDK frame callback logic.
-  These live in test_natnet_logic.cpp in the same test/ directory.
+C++ logic (``natnet_logic.hpp``) is tested in ``test_natnet_logic.cpp`` via colcon.
 """
 
 import sys
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 4ba91f58a..ea1693155 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -33,7 +33,7 @@ services:
         ipv4_address: 172.31.0.200 # required to not conflict with other default docker networks on the host machine
     env_file:
       - ./omni_pass.env
-      - ./px4-profiles/${PX4_PARAM_PROFILE:-default}.env
+      - ./sitl-files/${SITL_PARAM_PROFILE:-default}.env
     environment:
       - AUTOLAUNCH=${AUTOLAUNCH:-'false'}
       - DISPLAY=${DISPLAY}
@@ -43,9 +43,6 @@ services:
       - NUM_ROBOTS=${NUM_ROBOTS:-1}
       - ENABLE_LIDAR=${ENABLE_LIDAR:-false}
       - ISAAC_SIM_HEADLESS=${ISAAC_SIM_HEADLESS:-false}
-      # OptiTrack/NatNet emulator: mirror the robot-side LAUNCH_NATNET gate so the
-      # example launch scripts author a NatNet interface (one rigid body per drone
-      # base_link) and start the server on load when enabled.
       - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
       - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone}
       # Pegasus physics tuning — read by pegasus/simulator/params.py
diff --git a/simulation/isaac-sim/docker/px4-profiles/default.env b/simulation/isaac-sim/docker/px4-profiles/default.env
deleted file mode 100644
index 7d00f82cf..000000000
--- a/simulation/isaac-sim/docker/px4-profiles/default.env
+++ /dev/null
@@ -1,5 +0,0 @@
-# PX4 SITL parameter profile: default (no overrides beyond airframe + image patches).
-#
-# Selected by PX4_PARAM_PROFILE in the top-level .env (compose interpolation).
-# Variables here are injected into the isaac-sim container and applied at PX4 boot
-# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS.
\ No newline at end of file
diff --git a/simulation/isaac-sim/docker/sitl-files/default.env b/simulation/isaac-sim/docker/sitl-files/default.env
new file mode 100644
index 000000000..aac0ea21d
--- /dev/null
+++ b/simulation/isaac-sim/docker/sitl-files/default.env
@@ -0,0 +1,7 @@
+# SITL env bundle: default (no PX4_PARAM_* overrides beyond airframe + image patches).
+#
+# Selected by SITL_PARAM_PROFILE in the top-level .env (compose interpolation).
+# Variables here are injected into the isaac-sim container and applied at PX4 boot
+# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS.
+#
+# See px4-vision.env for external-vision / NatNet → MAVROS fusion tuning.
diff --git a/simulation/isaac-sim/docker/px4-profiles/vision.env b/simulation/isaac-sim/docker/sitl-files/px4-vision.env
similarity index 88%
rename from simulation/isaac-sim/docker/px4-profiles/vision.env
rename to simulation/isaac-sim/docker/sitl-files/px4-vision.env
index 3a38ed65e..686748d98 100644
--- a/simulation/isaac-sim/docker/px4-profiles/vision.env
+++ b/simulation/isaac-sim/docker/sitl-files/px4-vision.env
@@ -1,11 +1,11 @@
-# PX4 SITL parameter profile: external vision or state estimation.
+# PX4 SITL env bundle: external vision / mocap state estimation.
 #
 # Data path:
 #   Isaac NatNet emulator → natnet_ros2 → vision_pose_converter
 #   → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2
 #
 # Pair with robot-side publish_to_mavros: true in natnet_config.yaml.
-# Set PX4_PARAM_PROFILE=vision in .env (or overrides/isaac-natnet-vision.env).
+# Set SITL_PARAM_PROFILE=px4-vision in .env (or overrides/isaac-natnet-vision.env).
 
 # Fuse horizontal position, vertical position, and yaw from VISION_POSITION_ESTIMATE.
 # Bit 2 (velocity) omitted — pose_cov has no twist; MAVROS vision_pose is pose-only.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
index 3f6e890cd..0db35ef6e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore
@@ -1,8 +1,4 @@
 # OptiTrack SDK archives and build artifacts (reference tree may exist locally)
-NatNetClientSDK/*.zip
-NatNetClientSDK/**/x64/
-NatNetClientSDK/**/Release/
-NatNetClientSDK/**/Debug/
 **/*.obj
 **/*.pdb
 **/*.exe
@@ -11,6 +7,3 @@ NatNetClientSDK/**/Debug/
 **/*.tlog/
 **/__pycache__/
 **/*.pyc
-*.egg-info/
-dist/
-build/
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
index 17d99f52d..9ea3dc7cc 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
@@ -2,64 +2,153 @@
 
 Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/).
 
+The extension has two layers:
+
+1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration).
+2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers.
+
 ## Layout
 
 ```
 optitrack.natnet.emulator/
-├── config/extension.toml       # Isaac Sim extension manifest (stub)
+├── config/extension.toml          # Kit manifest (server module + UI entry point)
+├── schema/schema.usda             # Typed NatNet interface attribute definitions
 ├── setup.py
-├── optitrack/natnet/emulator/
-│   ├── defaults.py             # Test / Isaac reference constants (not used by server)
-│   └── server/                 # NatNet UDP server (transport + protocol)
-│       ├── natnet_server.py
-│       ├── natnet_unicast_server.py
-│       ├── natnet_data_types.py
-│       ├── natnet_model_types.py
-│       └── natnet_server_types.py
-└── NatNetClientSDK/            # Reference SDK only (not shipped in wheel)
+├── docs/
+│   └── natnet_interface_prim_design.md   # Design notes + implementation history
+├── test/                          # Co-located unit tests (proxied by tests/sim/)
+└── optitrack/natnet/emulator/
+    ├── defaults.py                # Reference Drone → prim bindings for tests
+    ├── server/                    # NatNet UDP server (transport + protocol)
+    │   ├── natnet_server.py       # Base server, queue, MODELDEF cache
+    │   ├── natnet_unicast_server.py
+    │   ├── natnet_data_types.py
+    │   ├── natnet_model_types.py
+    │   └── natnet_server_types.py
+    └── isaac/                     # Isaac Sim wrapper (Kit + USD)
+        ├── config.py              # Pure-Python NatNetInterfaceConfig model
+        ├── usd_bindings.py        # Author/read interface prims on a stage
+        ├── catalog.py             # Config → sDataDescriptions (MODELDEF)
+        ├── frames.py              # Prim poses → sFrameOfMocapData
+        ├── manager.py             # NatNetServerManager (lifecycle + sampling)
+        ├── scene_setup.py         # Pegasus launch helpers (start_drone_natnet_server)
+        └── ui_extension.py        # Docked editor panel (NatNetEmulatorExtension)
 ```
 
 ## Responsibilities
 
 | Layer | Role |
 |-------|------|
-| **Server** (`optitrack.natnet.emulator.server`) | UDP transport, `NAT_CONNECT` / `NAT_SERVERINFO`, MODELDEF **wire cache**, frame relay via `enqueue_mocap_data()` |
-| **Isaac wrapper** (planned) | Build catalog from scene config, `set_model_def_payload(catalog.pack())`, sample prims → `enqueue_mocap_data()` |
-| **`defaults.py`** | Hardcoded Drone → `/World/base_link` binding for tests and future Isaac wrapper; **not imported by the server** |
+| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. |
+| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). |
+| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. |
+
+The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config.
+
+## Stage-driven config prim
+
+Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes:
+
+- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise.
+- Bodies: multi-apply `natnet:body:<key>:*` fields mapping rigid-body name / streaming ID → target prim path.
+
+`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play.
+
+**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room.
+
+## Streaming model (Isaac)
 
-The server stores MODELDEF as packed **bytes** (`_model_def_payload`). It does not own prim-path bindings or ctypes catalog copies. The Isaac wrapper (or integration tests) call `set_model_def_payload()` after building `sDataDescriptions`.
+Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step:
 
-## Usage (development)
+1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`.
+2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread.
+
+Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop.
+
+Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network).
+
+## Enabling in AirStack
+
+**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
+
+**Isaac Sim:** same `LAUNCH_NATNET` env var (forwarded in [`docker-compose.yaml`](../../docker/docker-compose.yaml)). When true, the Pegasus **multi** launch script authors the interface prim and auto-starts the server via `start_drone_natnet_server()` — one rigid body per drone `base_link`. Single-drone harness uses `example_multi_px4_pegasus_launch_script.py` even for `NUM_ROBOTS=1`.
+
+Convenience bundle for NatNet + external-vision PX4 SITL:
+
+```bash
+airstack up --env-file overrides/isaac-natnet-vision.env
+```
+
+See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging.
+
+## Usage
+
+### Server only (no Kit)
 
 ```python
-from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType, make_default_drone_catalog
+from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog
+from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame
 
 server = NatNetUnicastServer(local_interface="172.31.0.200")
-# Default Drone MODELDEF is loaded on init; override from Isaac wrapper:
-# server.set_model_def_payload(make_default_drone_catalog().pack())
+server.set_model_def_payload(make_default_drone_catalog().pack())
 server.start()
-# Wrapper enqueues pre-built frames:
-# server.enqueue_mocap_data(frame)
+
+frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))])
+server.enqueue_mocap_data(frame)
+server.flush_mocap_data()
+```
+
+### Isaac launch script
+
+```python
+from optitrack.natnet.emulator.isaac import start_drone_natnet_server
+
+# Keep a reference to the manager for the sim lifetime.
+manager = start_drone_natnet_server(
+    stage,
+    drones=[("Drone", 1, "/World/drone1/base_link")],
+    server_ip="172.31.0.200",
+)
 ```
 
-Default Docker sim IP: `172.31.0.200` (Isaac container on AirStack bridge network).
+### Kit UI
+
+The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths.
+
+## Protocol notes (unicast, libNatNet 4.4)
+
+| Port | Traffic |
+|------|---------|
+| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo |
+| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port |
 
-## Protocol notes
+Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker.
 
-For libNatNet 4.4 **unicast**, the client uses a single UDP socket; server replies and frames go to the client's `NAT_CONNECT` source endpoint on the **command port** (1510). Handlers: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, `NAT_KEEPALIVE`. See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md).
+Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md).
 
 ## Tests
 
-| Tier | Mark | Location |
-|------|------|----------|
-| Unit (serializers, protocol, payload cache) | `unit` | `test/` + proxy in `tests/sim/optitrack_natnet_emulator/` |
-| Integration (natnet_ros2 loopback Hz) | `integration`, `natnet` | `tests/integration/natnet/test_natnet_integration.py` |
+| Tier | Mark | What |
+|------|------|------|
+| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup |
+| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz |
+
+Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/).
 
 ```bash
+# Unit (no Docker / no SDK)
 pytest tests/sim/optitrack_natnet_emulator/ -m unit -v
-pytest tests/integration/natnet/ -m natnet -v   # requires robot container + NatNet SDK
+
+# Integration (robot container + NatNet SDK)
+pytest tests/integration/natnet/ -m integration --run-integration -v
 ```
 
+Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`.
+
 ## Reference material
 
-Wire-format reference lives in [`NatNetClientSDK/`](NatNetClientSDK/README.md) (OptiTrack SDK samples; not redistributed by AirStack).
+- Design doc: [`docs/natnet_interface_prim_design.md`](docs/natnet_interface_prim_design.md)
+- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md)
+- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md)
+
+OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license).
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
index 8f05ef495..9a84e03f6 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
@@ -478,7 +478,7 @@ The extension already uses it today — test source in
    decorated `@pytest.mark.unit`.
 2. Add a one-line proxy
    `tests/sim/optitrack_natnet_emulator/test_<name>.py` that calls
-   `reexport_unit_tests(globals(), repo_path(".../optitrack.natnet.emulator/test"), "test_<name>.py")`.
+   `register_unit_tests(globals(), repo_path(".../optitrack.natnet.emulator/test"), "test_<name>.py")`.
 3. It is then discovered automatically by `pytest tests/ -m unit`,
    `airstack test -m unit`, and CI `system-tests.yml` (PR-open runs `pytest tests/`
    with no `-m` filter) — **no CI YAML change needed.**
@@ -582,7 +582,7 @@ Hard-won findings from getting the liveliness sentinel green (Isaac emulator →
    but never transmitted, so the client handshakes but receives nothing. Fix:
    `NatNetServer.auto_stream` (default `True`) gates that loop; the Isaac wrapper
    sets `auto_stream = False` in `manager.start_server()` and calls
-   `server.pump_once()` **synchronously from the physics-step callback**
+   `server.flush_mocap_data()` **synchronously from the physics-step callback**
    (`manager.sample_once()`). Outside Kit (host integration tests, sidecar) the
    default `auto_stream=True` path still works. Don't "optimize" the pump back into
    the daemon thread.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
index 2a0afd935..747831997 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
@@ -1,7 +1,4 @@
-"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.
-
-Not imported by NatNetServer — the server stores MODELDEF as packed bytes only.
-"""
+"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
index 9bb35e108..2812257f1 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -2,9 +2,8 @@
 # MIT License - see LICENSE in the repository root for full text.
 """Isaac Sim integration for the NatNet emulator (stage-driven config prim).
 
-``config`` is pure Python (no USD). ``usd_bindings`` imports ``pxr`` lazily, so
-importing this package is safe in non-Isaac environments; the USD functions only
-require ``pxr`` when actually called.
+``config`` is pure Python. ``usd_bindings`` imports ``pxr`` lazily, so
+importing this package is safe in non-Isaac environments.
 """
 
 from .config import (
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
index f83a0eb51..9c3bfaa35 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py
@@ -1,7 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commit 3 — catalog parse.
-
+"""
 Turn a :class:`NatNetInterfaceConfig` into the server's MODELDEF catalog
 (``sDataDescriptions`` of rigid bodies). Pure Python + ctypes (the ``server``
 package is stdlib-only), so this is hermetically unit-testable — no USD, no Kit.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
index b0c18b555..56c866ab1 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py
@@ -2,14 +2,12 @@
 # MIT License - see LICENSE in the repository root for full text.
 """Pure-Python config model for the stage-driven NatNet interface.
 
-No USD / Isaac imports here — this module is hermetically unit-testable. The USD
-binding layer (author/read against a ``Usd.Stage``) lives in ``usd_bindings.py``
-and depends on this model. Keeping the two split is the design's "design for
-testability" rule: nearly all logic stays free of ``pxr``.
+The USD binding layer (author/read against a ``Usd.Stage``) 
+lives in ``usd_bindings.py`` and depends on this model.
 
 Attribute names follow the multi-apply schema convention
-(``natnet:body:<key>:<field>``) so the custom-attribute backing used today is a
-drop-in for a future typed applied schema.
+(``natnet:body:<key>:<field>``).
+The custom-attribute backing is for a future typed applied schema.
 """
 
 from __future__ import annotations
@@ -45,10 +43,10 @@
 VALID_MODES = ("unicast", "multicast")
 
 # Streamed up-axis. Motive exposes an "Up Axis" setting; the reference natnet_ros2
-# driver requires it set to Z and passes coordinates through untouched. Isaac Sim /
-# USD is natively Z-up, so "Z" is a pass-through that matches the rest of the
+# driver requires it set to Z and passes coordinates through untouched.
+# Isaac Sim / USD is natively Z-up, so "Z" is a pass-through that matches the rest of the
 # AirStack stack (default). "Y" emulates a default (Y-up) Motive by rotating the
-# streamed pose -90deg about X. See ``frames.to_motive_pose``.
+# streamed pose -90deg about X.
 VALID_UP_AXES = ("Y", "Z")
 
 # defaults shared with NatNetUnicastServer
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
index cadcc95be..576969b80 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py
@@ -2,21 +2,19 @@
 # MIT License - see LICENSE in the repository root for full text.
 """Pose -> NatNet frame conversion (the data-enqueue path).
 
-Pure Python + ctypes (no USD, no Kit), so it's hermetically unit-testable. Sampled
-prim world poses become an ``sFrameOfMocapData`` of rigid bodies that the server
-streams to ``natnet_ros2``.
+Pure Python + ctypes. Sampled prim world poses become an 
+``sFrameOfMocapData`` of rigid bodies that the server 
+streams to the client.
 
 **Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2``
-(and the reference driver) require it set to **Z** and copy the rigid-body pose
-straight through (``rb_to_pose`` is an identity copy). Isaac Sim / USD is natively
-Z-up, so the default ``up_axis="Z"`` emits the prim's USD world pose **as-is** and
-stays in sync with the rest of the stack. ``up_axis="Y"`` emulates a default (Y-up)
-Motive — see :func:`to_motive_pose`.
+requires it set to **Z** and copy the rigid-body pose straight through 
+(``rb_to_pose`` is an identity copy). Isaac Sim is Z-up, so the default ``up_axis="Z"`` 
+emits the prim's USD world pose **as-is**. ``up_axis="Y"`` emulates a default (Y-up) 
+Motive by rotating the pose -90 deg about X.
 
 **params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``):
-``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it.
-``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF
-(set the frame after the catalog changes, e.g. a body added live).
+- ``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it.
+- ``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF(set the frame after the catalog changes, e.g. a body added live).
 """
 
 from __future__ import annotations
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
index 09f7b0891..398d761eb 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py
@@ -1,17 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commits 2-4 — detection, parameter read, and server start/stop lifecycle.
-
-``NatNetServerManager`` detects interface prims, prints their parsed config
-(Commit 2), and owns a **single** server instance it can start and stop (Commits
-3-4). On each enable it builds a MODELDEF catalog from the config and constructs a
-fresh server via an injectable factory (so unit tests can mock it and assert the
-server is created/started exactly once, without binding real sockets). Pose
-sampling / frame publishing is intentionally left for a later commit.
-
-``format_interface`` is pure (no USD). The server factory and lifecycle are
-USD-free too (they take a ``NatNetInterfaceConfig``); only the stage-driven
-helpers touch ``pxr`` / ``omni`` (lazily), so importing this module stays hermetic.
+"""
+``NatNetServerManager`` detects interface prims, samples poses from the stage,
+and owns a **single** server instance it can start and stop. 
+On each enable it builds a MODELDEF catalog from the config and constructs a
+fresh server via an injectable factory.
 """
 
 from __future__ import annotations
@@ -142,7 +135,7 @@ def _subscribe_physics(self):
             self._physx_sub = omni.physx.get_physx_interface().subscribe_physics_step_events(
                 self._on_physics_step
             )
-        except Exception as exc:  # pragma: no cover - Kit/physx only
+        except Exception as exc:  # Kit/physx only
             print(f"[natnet] Physics step subscription unavailable: {exc}")
             self._physx_sub = None
 
@@ -344,11 +337,10 @@ def sample_once(self, stage=None):
         )
         self._frame_counter += 1
         self._server.enqueue_mocap_data(frame)
-        # Send synchronously from this (physics-step) thread; the server's background
-        # data thread is unreliable inside the GIL-bound Isaac Sim process.
-        pump = getattr(self._server, "pump_once", None)
-        if callable(pump):
-            pump()
+        # Send synchronously from this (physics-step) thread.
+        flush_mocap_data = getattr(self._server, "flush_mocap_data", None)
+        if callable(flush_mocap_data):
+            flush_mocap_data()
         return frame
 
     # --- stage / USD notifications --------------------------------------------
@@ -382,20 +374,15 @@ def _revoke_usd_listener(self):
             self._usd_listener = None
 
     def _on_objects_changed(self, notice, sender):
-        # Only re-scan when something NatNet-related changed, so we don't spam the
-        # console on every transform update while the sim is playing.
+        # Only re-scan when something NatNet-related changed
         try:
             paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths())
-        except Exception:  # pragma: no cover - defensive
+        except Exception:
             paths = []
         if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths):
-            # A NatNet prim changed (e.g. a body added/retargeted while live): mark
-            # the sampler dirty so the next physics step re-reads the catalog and
-            # nudges the client to refresh MODELDEF.
+            # A NatNet prim changed: mark the sampler dirty so the next physics step re-reads the catalog.
             self._needs_resync = True
-            # A single author_interface() (Create/Save) emits many notices — one per
-            # attribute/relationship op. Debounce them into one scan on the next
-            # update tick so we print the final state once, not once per op.
+            # Debounce author_interface() calls into one scan on the next update tick.
             self._request_scan()
 
     def _request_scan(self):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
index 524c7f9b9..bf22065dd 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
@@ -95,12 +95,13 @@ def start_drone_natnet_server(
 
     Authors ``prim_path`` (overwriting any existing interface) with one rigid body
     per drone, then creates a :class:`NatNetServerManager` that subscribes to physics
-    steps and starts the server. Pump the sim (``timeline.play()`` / ``world.step``)
-    to stream poses. **Keep a reference to the returned manager** so it isn't
-    garbage-collected (which would tear down the physics subscription and server).
+    steps and starts the server. Play the sim to stream poses. Keep a reference to 
+    the returned manager so it isn't garbage-collected (which would tear 
+    down the physics subscription and stop the server).
 
-    Returns the ``NatNetServerManager``. If ``start`` is False (or the config is
-    authored disabled), the manager is created but the server is left stopped.
+    Returns the ``NatNetServerManager``. If ``start`` is 
+    False (or the config is authored disabled), 
+    the manager is created but the server is left stopped.
     """
     from .manager import NatNetServerManager
     from .usd_bindings import author_interface
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
index e83d742b6..50a581551 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py
@@ -2,28 +2,10 @@
 # MIT License - see LICENSE in the repository root for full text.
 """Kit extension entry: docked editor for the NatNet interface config prim.
 
-Commit 1 scope — create/manage ``/World/NatNetInterface``. The window docks to the
+Create/manage the ``/World/NatNetInterface`` prim. The window docks to the
 bottom-right (alongside the Property panel, like Pegasus) so it's easy to find.
 
-Sync model is **explicit and user-driven** via the top button row:
-
-* **Load from Stage** — read the prim into the window (or reset to defaults if none).
-* **Save** — write the window's current values to the prim (validate + author).
-* **Create Interface** — ensure the interface prim exists (authored from the current
-  values) and select it.
-* **Start/Stop Server** — start/stop the live UDP server directly (the button is the
-  authority, independent of ``serverEnabled``). While running, poses are sampled from
-  the USD stage on every physics step and streamed to ``natnet_ros2``.
-
-Each tracked-body row shows a live readout (status dot + world position pulled from
-the USD stage) so you can see at a glance what each body is publishing.
-
-Edits in the window mutate an in-memory working copy only; nothing touches the prim
-until you Save / Create, so you are in charge of keeping things synced.
-
-All ``omni.*`` imports live inside methods so this module never imports Kit at load
-time outside a running Kit process. Authoring goes through the tested, USD-only
-:mod:`optitrack.natnet.emulator.isaac.usd_bindings` facade.
+Sync model is explicit and user-driven via the top button row.
 """
 
 from __future__ import annotations
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
index 870534252..262216ee2 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py
@@ -2,14 +2,11 @@
 # MIT License - see LICENSE in the repository root for full text.
 """USD binding layer: author / read / find NatNet interface prims on a stage.
 
-``pxr`` is imported **lazily inside each function** so importing this module never
-requires USD — that keeps ``optitrack.natnet.emulator.isaac`` importable in plain
-(non-Isaac) test environments. The pure config logic lives in ``config.py``.
-
-Backing today is **plain namespaced custom attributes + relationships** (the
-registration-free fallback from the design's Risk #1). Property names follow the
-multi-apply schema convention (``natnet:body:<key>:<field>``), so swapping to a
-typed codeless applied schema later requires no change to readers/writers.
+``pxr`` is imported lazily inside each function so importing this module doesn't
+require USD.
+
+Backing today is plain namespaced custom attributes + relationships. Property names 
+follow the multi-apply schema convention (``natnet:body:<key>:<field>``).
 """
 
 from __future__ import annotations
@@ -87,9 +84,6 @@ def author_interface(stage, prim_path: str, config: Any) -> Any:
         _set(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), Sdf.ValueTypeNames.Int, body.streaming_id)
         _set(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), Sdf.ValueTypeNames.Int, body.parent_id)
         rel = prim.CreateRelationship(body_attr_name(key, BODY_FIELD_TARGET), False)
-        # Empty target is allowed (e.g. a freshly added body to be pointed in the
-        # Property panel); leave the relationship target-less rather than authoring
-        # an invalid empty Sdf.Path.
         rel.SetTargets([Sdf.Path(body.target_prim)] if body.target_prim else [])
 
     return prim
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
index 53e342bd5..91e23a412 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py
@@ -21,13 +21,13 @@ def pack(self) -> bytes:
 class sMarkerSetData(ctypes.Structure):
     _pack_ = 1
     _fields_ = [
-        ("szName", ctypes.c_char * ModelLimits.MAX_NAMELENGTH),            # MarkerSet name
-        ("nMarkers", ctypes.c_int32),                       # # of markers in MarkerSet
-        ("Markers", MarkerData * ModelLimits.MAX_MARKERS)   # Using max array, pack will chunk it
+        ("szName", ctypes.c_char * ModelLimits.MAX_NAMELENGTH), # MarkerSet name.
+        ("nMarkers", ctypes.c_int32),                           # Number of markers in MarkerSet.
+        ("Markers", MarkerData * ModelLimits.MAX_MARKERS)
     ]
 
     def pack(self) -> bytes:
-        # szName is null-terminated rather than fixed 256 over network
+        # szName is null-terminated on the wire.
         name_bytes = self.szName.rstrip(b'\x00') + b'\x00'
         payload = bytearray(name_bytes)
         payload += struct.pack('<i', self.nMarkers)
@@ -178,7 +178,7 @@ class sFrameOfMocapData(ctypes.Structure):
 
     @staticmethod
     def _pack_counted_section(count: int, data: bytes, *, natnet_major: int, natnet_minor: int) -> bytes:
-        """NatNet 4.1+ prefixes each collection with a 4-byte byte count (PacketClient UnpackDataSize)."""
+        """NatNet 4.1+ prefixes each collection with a 4-byte byte count."""
         payload = bytearray(struct.pack('<i', count))
         if (natnet_major == 4 and natnet_minor > 0) or natnet_major > 4:
             payload += struct.pack('<i', len(data))
@@ -255,9 +255,7 @@ def pack(self, natnet_major: int = 4, natnet_minor: int = 4) -> bytes:
         payload += struct.pack('<II', self.PrecisionTimestampSecs, self.PrecisionTimestampFractionalSecs)
         payload += struct.pack('<h', self.params)
 
-        # End-of-data tag. libNatNet's frame unpacker reads a trailing 4-byte
-        # tag after params; without it the unpacked size mismatches nDataBytes
-        # and the SDK silently drops the whole frame (no frame callback fires).
+        # End-of-data tag. libNatNet's frame unpacker reads a trailing 4-byte tag after params; 
         payload += struct.pack('<i', 0)
 
         return bytes(payload)
\ No newline at end of file
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
index 3795afd57..35b2daa14 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py
@@ -21,7 +21,7 @@ class sRigidBodyDescription(ctypes.Structure):
     _pack_ = 1
     _fields_ = [
         ("szName", ctypes.c_char * ModelLimits.MAX_NAMELENGTH),  # RigidBody name
-        ("ID", ctypes.c_int32),  # RigidBody identifier: Streaming ID value for rigid body assets, and Bone index value for skeleton rigid bodies.
+        ("ID", ctypes.c_int32),  # RigidBody identifier. Streaming ID value for rigid body assets, and Bone index value for skeleton rigid bodies.
         ("parentID", ctypes.c_int32),  # ID of parent Rigid Body (in case hierarchy exists; otherwise -1)
         ("offsetx", ctypes.c_float),  # offset position relative to parent
         ("offsety", ctypes.c_float),  # offset position relative to parent
@@ -77,10 +77,8 @@ def pack(self) -> bytes:
         return bytes(payload)
 
 
-# Tracked Object data description.
-# A Mocap Server application (e.g. Arena or TrackingTools) may contain multiple
-# tracked "objects (e.g. RigidBody, MarkerSet).  Each object will have its
-# own DataDescription.
+# Tracked Object data description. A Mocap Server application may contain multiple tracked "objects" 
+# (e.g. RigidBody, MarkerSet). Each object will have its own DataDescription.
 class sDataDescription(ctypes.Structure):
     _pack_ = 1
     _fields_ = [
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
index 1c1503b3a..05b3c0a2e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py
@@ -23,9 +23,7 @@ def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int]
         self.socket_lock = threading.Lock()
 
     def __hash__(self):
-        # We uniquely identify a client session by their IP and their unique command port.
-        # Why? A single machine could technically run multiple separate NatNet clients
-        # simultaneously, and they would share an IP but have unique command ports.
+        # Uniquely identify a client session by their IP and their unique command port.
         return hash((self.ip, self.port))
 
     def __eq__(self, other):
@@ -85,16 +83,13 @@ def __init__(self,
 
         self.running = False
 
-        # When True (default), the background data loop streams frames on its own
-        # timer. Set False when an external driver (the Isaac wrapper's physics-step
-        # callback) pumps frames synchronously via ``pump_once`` — inside the Isaac
-        # Sim process the daemon send thread is starved by the render/physics main
-        # loop holding the GIL, so frames must be sent from the callback thread.
+        # When True (default), the background data loop streams frames on its own timer. 
+        # Set False when an external driver (the Isaac wrapper's physics-step callback) 
+        # sends frames synchronously via ``flush_mocap_data``.
         self.auto_stream = True
 
-        # start() launches two daemon threads: a command listener (handshake /
-        # MODELDEF / keepalive) and a data loop that streams mocap frames. The
-        # transmission-specific behavior lives in the unicast/multicast subclass.
+        # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) 
+        # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass.
 
     def _signal_handler(self, signum, frame):
         print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...")
@@ -112,7 +107,7 @@ def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData):
         with self._last_mocap_lock:
             self._last_mocap_frame = new_data
 
-    def _get_last_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None:
+    def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None:
         with self._last_mocap_lock:
             return self._last_mocap_frame
 
@@ -149,8 +144,7 @@ def start(self):
 
         # 2. Setup Data Socket (Sends outward Mocap frames).
         # Bind to the data port so frames leave with source port == data_port.
-        # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port; frames
-        # arriving from the command port are treated as command traffic and dropped.
+        # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port
         self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
         self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
         self.data_socket.bind(('', self.data_port))
@@ -294,8 +288,7 @@ def _send_packet_to_client(
     ) -> None:
         """Send a NatNet packet to a unicast client (libNatNet 4.4).
 
-        Command replies go out the command socket; mocap frames go out the data
-        socket so their source port matches the advertised data port.
+        Command replies go out the command socket; mocap frames go out the data socket.
         """
         if self.shutdown_event.is_set():
             return
@@ -317,12 +310,12 @@ def _send_packet_to_client(
                 f"client {client.ip}:{client.port}: {e}"
             ) from e
 
-    def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
-        # Loop to update mocap data and send packets at regular intervals (e.g. 100Hz)
+    def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address)
+        # Loop to update mocap data and send packets at regular intervals.
         pass
 
     def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData):
-        # Serialize frame payload and send via the data socket (unicast libNatNet 4.4).
+        # Serialize frame payload and send via the data socket.
         try:
             packet_bytes = data_message.pack()
         except Exception as e:
@@ -335,11 +328,11 @@ def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfM
             sock=self.data_socket,
         )
 
-    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+    def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address)
         # Loop to listen for and handle incoming command requests (e.g. from client apps)
         pass
 
-    def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address)
+    def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address)
         # Parse incoming command request, perform requested action, and send response if needed
         pass
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
index f863ca087..d9c0a1e22 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py
@@ -20,30 +20,26 @@ def __init__(self,
         super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port)
 
     def _data_update_loop(self):
-        # Loop to update mocap data and send packets at regular intervals. When
-        # auto_stream is False the frames are pumped externally (Isaac physics step),
+        # Loop to update mocap data and send packets at regular intervals. 
+        # When auto_stream is False the frames are pumped externally (Isaac physics step),
         # so this thread only idles — but stays alive for clean shutdown.
         while not self.shutdown_event.is_set():
             time.sleep(1 / self.publish_rate)
             if not self.auto_stream:
                 continue
-            self.pump_once()
+            self.flush_mocap_data()
 
-    def pump_once(self):
-        """Send the latest (or last) mocap frame to every connected client, once.
-
-        Safe to call from any thread; the Isaac wrapper calls this from its
-        physics-step callback so frame delivery does not depend on the background
-        data thread getting scheduled inside the GIL-bound Isaac Sim process.
-        """
+    def flush_mocap_data(self):
+        """Send the latest (or last) mocap frame to every connected client, once."""
         with self.clients_lock:
             clients = list(self.connected_clients)
         if not clients:
             return
 
         data_messages = self._get_latest_mocap_packet()
-        if data_messages is None:
-            data_messages = self._get_last_mocap_frame()
+
+        if data_messages is None: # If the server stops producing frames, use the last known frame.
+            data_messages = self._get_last_known_mocap_frame()
         if data_messages is None:
             return
 
@@ -141,8 +137,7 @@ def _handle_command_request(self, request_data: bytes, client_address: tuple):
             return
 
         if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE):
-            # Keep-alive is client -> server only; real Motive sends no reply.
-            # Receiving it refreshes the client's liveness; nothing to send back.
+            # Receiving a keepalive refreshes the client's liveness; nothing to send back.
             return
 
         if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST):
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
index 1e4eeec76..6a15e5183 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py
@@ -1,13 +1,8 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Make the emulator package importable when running these tests directly.
+"""Put the extension root on sys.path for direct pytest test/ runs.
 
-The emulator is an Isaac Sim extension, not a pip-installed package, so a direct
-``pytest test/`` (or ``colcon test``) needs the extension root on ``sys.path``.
-pytest auto-loads this conftest before collecting any test in this directory, so
-the test modules themselves stay free of ``sys.path`` boilerplate. (Runs via
-``pytest tests/`` use the proxies under ``tests/`` instead, which set this up in
-``tests/conftest.py``.)
+CI uses proxies under tests/sim/optitrack_natnet_emulator/ instead.
 """
 
 import sys
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
index 407618965..a3cc8cd34 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py
@@ -1,10 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commit 3 — hermetic catalog-builder tests (no USD, no Kit).
-
-Covers no / single / multiple bodies, field fidelity on the wire, name truncation,
-the MAX_MODELS guard, and duplicate-target detection.
-"""
+"""Catalog builder: body counts, wire fidelity, truncation, MAX_MODELS, duplicate targets."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
index 642615e9f..11894c469 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py
@@ -1,6 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Light unit tests for Commit 2 detection/printing (the pure formatter)."""
+"""Unit tests for format_interface."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
index 10f4c8b4a..f495db67e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py
@@ -1,11 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""USD authoring/read round-trip tests for the NatNet interface prim.
-
-Guarded by ``pytest.importorskip("pxr")`` — these run wherever USD Python is
-available (the Isaac container, or CI when ``usd-core`` is installed from
-``tests/requirements.txt``) and skip cleanly otherwise.
-"""
+"""USD interface prim authoring and read round-trip (requires ``pxr``)."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
index fa13a7eb2..9f01ce80b 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py
@@ -1,11 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Pose sampling + dynamic catalog (dirty/resync) tests against an in-memory stage.
-
-Guarded by ``importorskip("pxr")``. Uses a fake server (records enqueued frames and
-MODELDEF payloads) so no sockets are bound — we exercise the manager's sampling and
-resync logic, not the wire.
-"""
+"""Pose sampling and catalog resync against an in-memory USD stage (fake server, no sockets)."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
index 84e2f9896..f93d761ab 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py
@@ -1,11 +1,8 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""End-to-end: manager samples a USD prim and streams its pose over the wire.
+"""Loopback: sample_once on a USD prim → NAT_FRAMEOFDATA with sampled position.
 
-Real server + real UDP sockets (loopback) + an in-memory USD stage. Proves the full
-data-enqueue path: ``sample_once`` reads the prim's world pose, builds a frame, the
-server relays it as ``NAT_FRAMEOFDATA``, and the bytes carry the sampled position.
-Guarded by ``importorskip("pxr")``.
+Real server + UDP sockets + in-memory stage. Requires pxr.
 """
 
 from __future__ import annotations
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
index 2339cd9fc..a6ffd5b14 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py
@@ -1,12 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commit 3 — end-to-end: manager builds a real server from a config.
-
-Functional loopback (real UDP sockets, default factory): the manager starts a
-server from a multi-body config, a NatNet client connects, requests MODELDEF, and
-the served catalog must match ``build_catalog(config)``. Then a clean stop must
-free the port so a fresh server can re-bind it.
-"""
+"""Manager → real ``NatNetUnicastServer``: MODELDEF handshake and frame delivery."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
index 549903b19..aa2921e14 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py
@@ -1,12 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commits 3-4 — server lifecycle tests with a mocked server factory.
-
-These never bind a socket: a fake factory records construction/start/shutdown so we
-can assert the manager creates and starts the server exactly once, is idempotent,
-toggles, and reconciles to ``serverEnabled``. No USD, no Kit.
-"""
-
+"""Server lifecycle tests with a mocked server factory."""
 from __future__ import annotations
 
 import pytest
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
index b7afa1e91..0d38a295e 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py
@@ -1,12 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Commit 3 — target prim resolution against a real (in-memory) USD stage.
-
-Guarded by ``importorskip("pxr")`` so the suite stays green without usd-core.
-Covers no / single / multiple bodies, missing prims, empty targets, and multiple
-catalog entries pointing at the same prim.
-"""
-
+"""Target resolution against a real (in-memory) USD stage."""
 from __future__ import annotations
 
 import pytest
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
index 0414e7f2f..fe8b114f8 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py
@@ -1,6 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""UDP loopback protocol tests for NatNetUnicastServer."""
+"""Unicast protocol tests for NatNetUnicastServer."""
 
 from __future__ import annotations
 
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
index 7baca6ee3..2fdd21e80 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
@@ -39,10 +39,7 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
-# Make the OptiTrack NatNet emulator package importable without enabling the Kit
-# UI extension (keeps a single, script-owned server manager).
-sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator")))
-
+from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
 # --------------------- CONFIGURATION ---------------------
 ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
@@ -186,7 +183,7 @@ def __init__(self):
 
         # ----- OptiTrack / NatNet emulator -----
         # Author a NatNet interface prim with every drone's base_link as a tracked
-        # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET).
+        # rigid body and start the Motive-compatible server.
         self.natnet_manager = None
         if LAUNCH_NATNET:
             self._setup_natnet(stage)
@@ -196,8 +193,6 @@ def __init__(self):
     def _setup_natnet(self, stage):
         """Author the NatNet interface prim (one body per drone) and start the server."""
         try:
-            from optitrack.natnet.emulator.isaac import start_drone_natnet_server
-
             # Single agent: stream the bare body name ("Drone") so it matches the
             # robot natnet_config body_name, the MAVROS vision-pose bridge, and the
             # liveliness sentinel. Multi-drone uses unique indexed names (future:
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
index fadbc0682..f4dad4f89 100755
--- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
@@ -97,10 +97,7 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
-# Make the OptiTrack NatNet emulator package importable without enabling the Kit
-# UI extension (keeps a single, script-owned server manager).
-sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator")))
-
+from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
 # --------------------- CONFIGURATION ---------------------
 # Environment to load. Swap this URL/key for any other scene.
@@ -257,7 +254,7 @@ def __init__(self):
 
         # ----- OptiTrack / NatNet emulator -----
         # Author a NatNet interface prim with the drone's base_link as a tracked
-        # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET).
+        # rigid body and start the Motive-compatible server.
         self.natnet_manager = None
         if LAUNCH_NATNET:
             self._setup_natnet(stage)
@@ -267,8 +264,6 @@ def __init__(self):
     def _setup_natnet(self, stage):
         """Author the NatNet interface prim and start the emulator server."""
         try:
-            from optitrack.natnet.emulator.isaac import start_drone_natnet_server
-
             drones = [(NATNET_BODY_NAME, 1, "/World/base_link/body")]
             self.natnet_manager = start_drone_natnet_server(
                 stage, 
diff --git a/tests/README.md b/tests/README.md
index e8c5b1e89..7a992ac7d 100644
--- a/tests/README.md
+++ b/tests/README.md
@@ -33,7 +33,7 @@ See [`integration/README.md`](integration/README.md).
 
 | Scenario | Mark(s) | What it tests | Hardware required |
 |----------|---------|---------------|-------------------|
-| [`integration/natnet/`](integration/natnet/) | `integration`, `natnet` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim) |
+| [`integration/natnet/`](integration/natnet/) | `integration` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim); runs after `build_docker` / `build_packages` in collection order |
 
 The shared `robot_autonomy_stack` fixture (in [`conftest.py`](conftest.py))
 reuses a running `robot-desktop` container, or brings one up only when
@@ -52,8 +52,9 @@ Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)).
 
 **Co-location + proxy pattern:** test source lives alongside its ROS 2 package at
 `robot/ros_ws/src/<layer>/<package>/test/test_*.py` (the ROS 2 / colcon convention).
-Files in `tests/robot/` are thin proxies that re-export those tests so that
-`pytest tests/` discovers them. Both `airstack test -m unit` and
+Files in `tests/robot/` are thin proxies that call ``register_unit_tests()`` (in
+[`conftest.py`](conftest.py)) to register co-located tests so ``pytest tests/``
+discovers them. Both `airstack test -m unit` and
 `colcon test --packages-select <pkg>` run the same test source.
 
 Example: `robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py`
diff --git a/tests/conftest.py b/tests/conftest.py
index d7ff96f4c..a8889f1e8 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -62,16 +62,35 @@ def repo_path(*parts: str) -> Path:
     return Path(AIRSTACK_ROOT).joinpath(*parts)
 
 
-def reexport_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None:
-    """Exec co-located unit-test modules and re-export their ``test_*`` callables.
-
-    Lets the thin proxy files under ``tests/`` expose package-co-located unit
-    tests to ``pytest tests/`` (CI) and ``airstack test -m unit`` without
-    per-file ``sys.path`` boilerplate. The package root (``test_dir.parent``)
-    and the test dir are placed on ``sys.path`` so the exec'd modules can import
-    the package under test and any sibling helper modules. Every re-exported
-    test is tagged ``unit`` so selection works regardless of whether the source
-    used a module-level ``pytestmark`` or per-function marks.
+def register_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None:
+    """Register co-located unit tests with a thin proxy module under ``tests/``.
+
+    AirStack keeps unit test **source** next to each package (colcon convention),
+    but CI runs pytest tests/. Each proxy file calls this helper so pytest
+    discovers the real tests.
+
+    For each module_file in test_dir:
+
+    1. Prepend test_dir and test_dir.parent (package/extension root) to
+       sys.path so the loaded module can import production code and sibling
+       helpers (e.g. natnet_test_helpers).
+    2. Load the file with importlib.util.spec_from_file_location under a
+       synthetic module name (_unit_<package>_<stem>) to avoid circular
+       imports when proxy and source share the same basename.
+    3. Copy every test_* callable from the loaded module into
+       target_globals (typically the proxy's globals()) so pytest
+       collects them as if they lived under tests/.
+    4. Wrap each callable with pytest.mark.unit so -m unit works even
+       when the source module omits pytestmark.
+
+    Args:
+        target_globals: Namespace to populate (pass globals() from the proxy).
+        test_dir: Directory containing co-located test modules
+            (e.g. repo_path("robot/ros_ws/src/<pkg>/test")).
+        *module_files: One or more filenames under test_dir; multiple files
+            may be folded into a single proxy.
+
+    pytest <package>/test/ or colcon test bypasses proxies; use a local conftest.py in the package test/ dir for sys.path setup instead.
     """
     for path in (test_dir, test_dir.parent):
         entry = str(path)
@@ -122,9 +141,9 @@ def colcon_test_robot_command(workspace="robot"):
         cmd += f' --pytest-args "{pytest_args}"'
     return cmd
 # Unit tests live co-located with their ROS 2 packages in robot/ros_ws/src/.
-# Thin proxy files under tests/robot/ re-export those tests so that
-# `pytest tests/` and `airstack test -m unit` discover them without any
-# sys.path manipulation here.  Each proxy file sets up its own paths.
+# Thin proxy files under tests/robot/ (etc.) call register_unit_tests() so
+# `pytest tests/` and `airstack test -m unit` discover them without sys.path
+# boilerplate in this root conftest.
 RUN_DIR = None
 LOGS_DIR = None
 ROS_DISTRO_SETUP = "/opt/ros/jazzy/setup.bash"
@@ -251,16 +270,17 @@ def pytest_generate_tests(metafunc):
 
 
 # Run cheap/fast-fail tests first so real problems surface early:
-# docker image builds → colcon workspace builds → liveliness (infra) → sensors
-# (ROS topic streams) → autonomy flight tests.
+# unit → docker image builds → colcon builds → integration (robot container) →
+# liveliness (full stack infra) → sensors → autonomy flight tests.
 _MODULE_ORDER = [
     # Unit tests first — fast, hermetic, no Docker.  Any module whose dotted
     # name starts with "robot." or "sim." is a proxy for a package-level unit
     # test and sorts into this leading slot via the prefix check below.
     "__unit__",
-    # System tests follow in dependency order.
     "system.test_build_docker",
     "system.test_build_packages",
+    # Integration needs robot-desktop image + colcon-built packages (e.g. natnet_ros2).
+    "__integration__",
     "system.test_liveliness",
     "system.test_sensors",
     "system.test_takeoff_hover_land",
@@ -283,12 +303,14 @@ def _rank(name, order):
 def _module_key(item):
     """Return the ordering key for an item.
 
-    Unit-test proxies live under ``robot/``, ``sim/``, or ``gcs/`` and are
-    identified by their nodeid prefix.  Everything else uses the dotted module
-    ``__name__`` looked up against ``_MODULE_ORDER``.
+    Unit-test proxies live under robot/, sim/, or gcs/.
+    Integration tests live under integration/.
+    Everything else uses the dotted module __name__ against _MODULE_ORDER.
     """
     if item.nodeid.startswith(("robot/", "sim/", "gcs/")):
         return _rank("__unit__", _MODULE_ORDER)
+    if item.nodeid.startswith("integration/"):
+        return _rank("__integration__", _MODULE_ORDER)
     return _rank(getattr(item.module, "__name__", ""), _MODULE_ORDER)
 
 
diff --git a/tests/docker/docker-compose.yaml b/tests/docker/docker-compose.yaml
index 28bde3275..841788917 100644
--- a/tests/docker/docker-compose.yaml
+++ b/tests/docker/docker-compose.yaml
@@ -13,9 +13,6 @@ services:
     environment:
       - AIRSTACK_ROOT=${AIRSTACK_PATH}
       - DISPLAY=${DISPLAY:-}
-      # Forward the OptiTrack/NatNet toggle so the in-runner `airstack up`
-      # (which does os.environ.copy()) brings the stack up with NatNet enabled and
-      # the test_natnet_pose_alive sentinel runs instead of skipping.
       - LAUNCH_NATNET=${LAUNCH_NATNET:-false}
       - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone}
     working_dir: ${AIRSTACK_PATH}/tests
diff --git a/tests/integration/README.md b/tests/integration/README.md
index 8db63f3d1..14189245b 100644
--- a/tests/integration/README.md
+++ b/tests/integration/README.md
@@ -5,12 +5,13 @@ The **integration** tier sits between **unit** and **system**:
 | Tier | Lives in | Brings up | Hardware | Mark |
 |------|----------|-----------|----------|------|
 | unit | `<pkg>/test/` + proxies in `tests/robot/`, `tests/sim/`, `tests/gcs/` | nothing | none | `unit` |
-| **integration** | **`tests/integration/<scenario>/`** | **robot container + a host-side component** | **Docker (no sim/GPU)** | **`integration`** (+ a specific mark) |
+| **integration** | **`tests/integration/<scenario>/`** | **robot container + a host-side component** | **Docker (no sim/GPU)** | **`integration`** |
 | system | `tests/system/` | full sim + robot + GCS | Docker + GPU + sim license | `liveliness`, `sensors`, `takeoff_hover_land` |
 
 An integration test wires a few **real** components together — for example the
-robot autonomy stack plus a host-side NatNet server — without paying for a full
-simulator or GPU.
+robot autonomy stack plus a host-side NatNet server — without using a full simulator or GPU.
+
+**Collection order** (see `pytest_collection_modifyitems` in [`../conftest.py`](../conftest.py)):
 
 ## The harness: `robot_autonomy_stack`
 
@@ -31,6 +32,9 @@ defined there). Request it instead of hand-rolling `airstack up`:
 AUTOLAUNCH=false airstack up robot-desktop
 pytest tests/integration/ -m integration -v
 
+# One scenario by path:
+pytest tests/integration/natnet/ -m integration -v
+
 # Or let the harness bring the robot container up/down itself:
 pytest tests/integration/ -m integration --run-integration -v
 
@@ -41,13 +45,12 @@ pytest tests/integration/ -m integration --run-integration -v
 ## Adding a scenario
 
 1. Create `tests/integration/<scenario>/test_*.py`.
-2. `pytestmark = [pytest.mark.integration, pytest.mark.<scenario>]` — the
-   `integration` umbrella selects the whole tier; the specific mark targets one.
+2. Set `pytestmark = pytest.mark.integration`.
 3. Request the `robot_autonomy_stack` fixture for the container.
-4. Register the specific mark in [`../pytest.ini`](../pytest.ini).
+4. Filter by path (`tests/integration/<scenario>/`) when you only want that scenario.
 
 ## Residents
 
-| Scenario | Mark | What it verifies |
-|----------|------|------------------|
-| [`natnet/`](natnet/) | `natnet` | Host NatNet emulator → `natnet_ros2` → pose topic Hz |
+| Scenario | What it verifies |
+|----------|------------------|
+| [`natnet/`](natnet/) | Host NatNet emulator → `natnet_ros2` → pose topic Hz |
diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md
index 7e8919aa3..74502273f 100644
--- a/tests/integration/natnet/README.md
+++ b/tests/integration/natnet/README.md
@@ -1,82 +1,113 @@
 # NatNet ↔ robot autonomy integration
 
-Drives the Python NatNet wire-protocol emulator against `natnet_ros2_node`
-running with the robot autonomy stack. First resident of the
-[`integration`](../README.md) tier.
+Host-side NatNet wire-protocol tests that drive the Python emulator against
+`natnet_ros2_node` in a real robot container. First resident of the
+[`integration`](../README.md) tier (no sim, no GPU).
 
-Marks: `integration` (tier) + `natnet` (scenario).
+Mark: `integration`. Filter this scenario with `tests/integration/natnet/`.
+
+For the **in-sim** end-to-end check (Isaac emulator + full stack), see
+[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and
+[`tests/system/test_liveliness.py`](../../system/test_liveliness.py).
 
 ## What it verifies
 
-Two variants, both ending at `/{ROBOT_NAME}/perception/optitrack/Drone` ≥ 5 Hz:
+Two variants in [`test_natnet_integration.py`](test_natnet_integration.py).
+Both start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the
+robot container pointed at the Docker bridge gateway, and assert a sustained
+pose stream at **≥ 5 Hz** on:
+
+- `/{ROBOT_NAME}/perception/optitrack/Drone/pose_cov` (wait for first message)
+- `/{ROBOT_NAME}/perception/optitrack/Drone` (Hz sample)
 
-1. **Raw server** (`test_natnet_ros2_receives_drone_pose_hz`) — hand-built frames
-   via `NatNetUnicastServer` (no USD): the minimal end-to-end wire check.
-2. **Isaac wrapper** (`test_natnet_ros2_receives_isaac_wrapper_pose_hz`) — the full
-   new data path: `NatNetServerManager` builds the catalog from a
-   `NatNetInterfaceConfig` and samples a moving prim's world pose off an in-memory
-   USD stage (`sample_once`, exactly what the in-sim physics-step callback does),
-   streaming real frames to the robot client. Skips without `usd-core` (`pxr`).
-   Exact pose-value fidelity is covered hermetically by the package's
-   `test_pose_streaming.py` loopback.
+| Test | Path |
+|------|------|
+| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. |
+| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. |
+
+These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they
+exec `natnet_ros2_node` directly with `body_name:=Drone`, `body_id:=1`, and
+`publish_to_mavros:=false`.
 
 ## Requirements
 
-- A NatNet UDP server (`NatNetUnicastServer`) on the host — started by the test.
-- `natnet_ros2_node` (OptiTrack NatNet SDK) built in the robot container.
-- Docker bridge routing from container → host gateway.
+- Docker daemon (robot-desktop container reachable from pytest).
+- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is
+  license-gated — run `airstack setup --natnet`, then
+  `bws --packages-select natnet_ros2` in the container). Tests **skip** if the
+  node binary is missing.
+- Host-side emulator package on `PYTHONPATH` (the test adds
+  `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed
+  on the host).
+- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from
+  inside the container).
 
-The robot container is provided by the `robot_autonomy_stack` fixture (see the
-[tier README](../README.md)). The test **skips** when `natnet_ros2_node` is not
-built (the SDK is license-gated, fetched via `airstack setup --natnet`).
+The robot container comes from the shared **`robot_autonomy_stack`** fixture in
+[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)).
 
 ## Running
 
 ```bash
-# Reuse an existing robot container:
+# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image
+airstack setup --natnet   # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet
+docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2'
+
+# 2a. Reuse an existing robot container (fast local iteration):
 AUTOLAUNCH=false airstack up robot-desktop
-pytest tests/integration/natnet/ -m natnet -v
+pytest tests/integration/natnet/ -m integration -v
 
-# Or let the harness bring the container up/down:
-pytest tests/integration/natnet/ -m natnet --run-integration -v
+# 2b. Let the harness bring the container up/down:
+pytest tests/integration/natnet/ -m integration --run-integration -v
 ```
+On CI / PR (write access): `/pytest -m integration --run-integration`
 
 ## Architecture
 
 ```
-┌─────────────────────────────────────────────────────────────┐
+┌──────────────────────────────────────────────────────────────┐
 │ Host (pytest)                                                │
-│  NatNetUnicastServer — MODELDEF bytes cache + frame queue   │
-└───────────────────────────┬─────────────────────────────────┘
-                            │ UDP (docker bridge gateway IP)
-┌───────────────────────────▼─────────────────────────────────┐
-│ Robot container (autonomy stack)                             │
-│  natnet_ros2_node (libNatNet 4.4 unicast)                   │
-│  → /{ROBOT_NAME}/perception/optitrack/Drone                 │
-└─────────────────────────────────────────────────────────────┘
+│  NatNetUnicastServer @ docker bridge gateway IP              │
+│    • raw variant: hand-built frame queue                     │
+│    • Isaac variant: NatNetServerManager.sample_once(USD)     │
+└────────────────────────────┬─────────────────────────────────┘
+                             │ UDP unicast (cmd + data ports)
+┌────────────────────────────▼─────────────────────────────────┐
+│ Robot container (robot-desktop)                              │
+│  natnet_ros2_node (libNatNet 4.4 client)                     │
+│  → /{ROBOT_NAME}/perception/optitrack/Drone[/pose_cov]       │
+└──────────────────────────────────────────────────────────────┘
 ```
 
-**Catalog ownership:** The server holds a MODELDEF **wire cache** only. Scene
-semantics (prim paths, body names/IDs) belong in the future Isaac Sim wrapper,
-which calls `set_model_def_payload()`. See
-[`defaults.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py)
-for hardcoded Drone reference constants used in tests.
+**In sim (liveliness tier):** the server runs inside the Isaac Sim container
+(`172.31.0.200` by default). Pegasus launch scripts call
+`start_drone_natnet_server()` when `LAUNCH_NATNET=true`; `natnet_ros2` in the
+robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP).
+
+**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only
+(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target
+prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface
+prim, or launch-script `build_drone_config`). See the
+[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md).
 
 ## Liveliness sentinel (sim end-to-end)
 
-The integration tier drives the emulator **host-side** (no sim/GPU). The matching
-in-sim check is a conditional sentinel in
-[`../../system/test_liveliness.py`](../../system/test_liveliness.py):
-`TestLiveliness::test_natnet_pose_alive` asserts
-`/{robot}/perception/optitrack/<body>` is live per robot. It is **gated on
-`LAUNCH_NATNET=true`** (skips otherwise), so normal liveliness runs are unaffected.
-Override the body name with `NATNET_BODY_NAME` (default `Drone`).
+The integration tier proves **robot client + host emulator** without Isaac.
+The matching **system** check is
+`TestLiveliness::test_natnet_pose_alive` in
+[`test_liveliness.py`](../../system/test_liveliness.py):
+
+- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs
+  are unaffected).
+- Asserts `/{robot_n}/perception/optitrack/{body}/pose_cov` ≥ 5 Hz per robot.
+- Override body name with `NATNET_BODY_NAME` (default `Drone`; multi-drone sim
+  uses indexed names like `Drone1` — align with
+  `example_multi_px4_pegasus_launch_script.py` and per-robot `natnet_config`).
 
-> The sentinel passes once the Isaac emulator actually streams in-sim — i.e. a
-> Pegasus launch script authors a `NatNetInterface` prim and calls
-> `NatNetServerManager.start_from_stage()` (the scripting entry point). Until that
-> sim auto-start is wired, run the **integration** variants above for robot-level
-> coverage.
+Sim auto-start is wired: with `LAUNCH_NATNET=true`, the Pegasus **multi** launch
+script (used by the system-test harness even for `NUM_ROBOTS=1`) authors
+`/World/NatNetInterface` and starts `NatNetServerManager` via
+`start_drone_natnet_server()`. Convenience env bundle:
+`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet + PX4 external-vision SITL profile).
 
 ## libNatNet 4.4 unicast — verified wire contract
 
@@ -100,5 +131,19 @@ With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`,
 > and *on the command port*, which is why it appeared to work while libNatNet did
 > not. Always validate against the C SDK.
 
-Rebuild `natnet_ros2` in the robot container after any adapter changes:
-`docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2'`
+Full handshake notes and sniffing workflow:
+[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md).
+
+## After changing natnet_ros2 or the emulator
+
+Rebuild in the robot container:
+
+```bash
+docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2'
+```
+
+Unit tests (protocol, serializers, Isaac wrapper loopback):
+
+```bash
+pytest tests/sim/optitrack_natnet_emulator/ -m unit -v
+```
diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py
index 6a98551c1..427be2d78 100644
--- a/tests/integration/natnet/test_natnet_integration.py
+++ b/tests/integration/natnet/test_natnet_integration.py
@@ -1,15 +1,13 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""NatNet emulator ↔ robot autonomy integration test.
+"""NatNet - robot autonomy integration tests.
 
-Wide-scale integration: a host-side NatNet server (the Python emulator) streams
-dummy Drone frames to ``natnet_ros2`` running against the robot autonomy stack,
-and we assert the pose topic stays alive at a stable rate.
+Two host-side variants stream Drone frames to ``natnet_ros2_node`` in the robot
+container and assert pose topics stay alive at >= 5 Hz: (1) raw
+``NatNetUnicastServer`` hand-built frames; (2) ``NatNetServerManager`` sampling
+an in-memory USD stage (Isaac wrapper path, no sim/GPU).
 
-This is the first resident of the ``integration`` tier. Today the NatNet
-*server* is the host emulator; once the Isaac-sim emulator wrapper emits NatNet
-frames, an Isaac-wrapped variant will be added here, and the gated pose-rate
-check can additionally surface in ``system/test_liveliness.py``.
+In-sim end-to-end: ``tests/system/test_liveliness.py::test_natnet_pose_alive``.
 """
 
 from __future__ import annotations
@@ -29,8 +27,7 @@
     wait_for_first_message,
 )
 
-# Emulator package (host-side) is not pip-installed; expose it + its test
-# helpers via the AIRSTACK_ROOT-anchored repo_path() (works in CI and locally).
+# Emulator is not pip-installed on the host; add extension root + test helpers.
 _EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator")
 for _path in (_EXT_ROOT, _EXT_ROOT / "test"):
     if str(_path) not in sys.path:
@@ -40,7 +37,7 @@
 from optitrack.natnet.emulator.server import natnet_data_types as dt  # noqa: E402
 from natnet_test_helpers import ephemeral_udp_port  # noqa: E402
 
-pytestmark = [pytest.mark.integration, pytest.mark.natnet]
+pytestmark = pytest.mark.integration
 
 _ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash"
 _NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node"
@@ -160,11 +157,7 @@ def _terminate(proc) -> None:
 
 
 def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
-    """Emulator on host streams dummy Drone frames while natnet_ros2_node publishes.
-
-    Raw-server variant: hand-built frames via ``NatNetUnicastServer`` (no Isaac
-    wrapper, no USD) — the minimal end-to-end wire check.
-    """
+    """Raw-server path: hand-built frames on NatNetUnicastServer."""
     container = robot_autonomy_stack["container"]
 
     if not _natnet_node_available(container):
@@ -209,15 +202,10 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack):
 
 
 def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack):
-    """Isaac-wrapper variant: the full new data path drives natnet_ros2.
-
-    ``NatNetServerManager`` builds the catalog from a ``NatNetInterfaceConfig``,
-    samples a (moving) prim's world pose off an in-memory USD stage via
-    ``sample_once`` — exactly what the physics-step callback does in-sim — and
-    streams real frames. We assert ``natnet_ros2`` connects and publishes the pose
-    at a stable rate. (Exact pose-value fidelity is covered hermetically by the
-    package's ``test_pose_streaming.py`` loopback; here we prove the wrapper feeds
-    the *real* robot client end to end without a sim/GPU.)
+    """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim.
+
+    Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity
+    is covered by test_pose_streaming.py loopback.
     """
     pytest.importorskip("pxr")
     import math
diff --git a/tests/pytest.ini b/tests/pytest.ini
index f5220f93a..6e7bd7c6b 100644
--- a/tests/pytest.ini
+++ b/tests/pytest.ini
@@ -3,11 +3,10 @@ markers =
     unit: Fast hermetic tests (no Docker stack; numpy / pure Python)
     build_docker: Docker image build tests
     build_packages: Colcon workspace build tests
+    integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU)
     liveliness: Container and process health (Docker, tmux, sentinel ROS 2 nodes)
     sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF
     takeoff_hover_land: End-to-end takeoff / hover / land action tests
-    integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU)
-    natnet: NatNet emulator ↔ natnet_ros2 integration (robot container + UDP server)
 testpaths = .
 addopts = -v --durations=0
 cache_dir = /tmp/.pytest_cache
diff --git a/tests/robot/README.md b/tests/robot/README.md
index a4409c4b1..2c7806367 100644
--- a/tests/robot/README.md
+++ b/tests/robot/README.md
@@ -20,13 +20,13 @@ convention):
 robot/ros_ws/src/<layer>/<package>/test/test_<name>.py   ← source of truth
 ```
 
-**This directory** contains thin proxy files that load the real test module via
-`importlib` and re-export its `test_*` functions, making them discoverable by
-`pytest tests/` and `airstack test -m unit` without any changes to the CI
-workflow. Each proxy is ~15 lines.
+**This directory** contains thin proxy files that call ``register_unit_tests()``
+(from [`tests/conftest.py`](../conftest.py)) to register co-located ``test_*``
+functions for ``pytest tests/`` and ``airstack test -m unit``. Each proxy is
+~10 lines.
 
 ```
-tests/robot/<layer>/<package>/test_<name>.py             ← proxy (re-exports above)
+tests/robot/<layer>/<package>/test_<name>.py             ← proxy (registers above)
 ```
 
 Both `airstack test -m unit` (pytest path via proxy) and
diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
index aa1f2023d..a8bcb1dfb 100644
--- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
+++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py
@@ -1,19 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes natnet_ros2 unit tests from the package source tree.
+"""Proxy: registers natnet_ros2 Python unit tests for pytest tests/."""
 
-Unit test logic lives co-located with its package (ROS 2 / colcon convention):
-  robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
+from conftest import register_unit_tests, repo_path
 
-This file makes those tests discoverable by ``pytest tests/`` (CI) and
-``airstack test -m unit`` without any changes to the CI workflow.
-Run ``colcon test --packages-select natnet_ros2`` to also execute the C++
-gtests and ament linters.
-"""
-
-from conftest import reexport_unit_tests, repo_path
-
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("robot/ros_ws/src/perception/natnet_ros2/test"),
     "test_natnet_ros2.py",
diff --git a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
index 7af5ab631..677680092 100644
--- a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
+++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py
@@ -1,6 +1,6 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes validation_core unit tests from the package source tree.
+"""Proxy: registers validation_core unit tests from the package source tree.
 
 Unit test logic lives co-located with its package (ROS 2 / colcon convention):
   robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py
@@ -11,9 +11,9 @@
 the ament linters.
 """
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("robot/ros_ws/src/sensors/lidar_point_cloud_filter/test"),
     "test_validation_core.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_catalog.py b/tests/sim/optitrack_natnet_emulator/test_catalog.py
index 0d19bf34b..8bb6b4730 100644
--- a/tests/sim/optitrack_natnet_emulator/test_catalog.py
+++ b/tests/sim/optitrack_natnet_emulator/test_catalog.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 catalog-builder tests."""
+"""Proxy: registers NatNet emulator catalog-builder unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_catalog.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py
index 0dca746d2..c598d25f0 100644
--- a/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py
+++ b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator default/catalog unit tests for pytest tests/."""
+"""Proxy: registers optitrack.natnet.emulator default/catalog unit tests for pytest tests/."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_defaults.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_discovery.py b/tests/sim/optitrack_natnet_emulator/test_discovery.py
index b1d43de2b..d0559b68f 100644
--- a/tests/sim/optitrack_natnet_emulator/test_discovery.py
+++ b/tests/sim/optitrack_natnet_emulator/test_discovery.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator Commit 2 detection unit tests."""
+"""Proxy: registers NatNet emulator format_interface unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_discovery.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_frames.py b/tests/sim/optitrack_natnet_emulator/test_frames.py
index 493abb722..7a95f783e 100644
--- a/tests/sim/optitrack_natnet_emulator/test_frames.py
+++ b/tests/sim/optitrack_natnet_emulator/test_frames.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator pose->frame builder unit tests."""
+"""Proxy: registers optitrack.natnet.emulator pose->frame builder unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_frames.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
index 9c8271bd9..56a4d410c 100644
--- a/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
+++ b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py
@@ -1,14 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator USD authoring unit tests.
+"""Proxy: registers optitrack.natnet.emulator USD authoring unit tests (skips without ``pxr``)."""
 
-The underlying tests `pytest.importorskip("pxr")`, so they skip unless USD Python
-(`usd-core` in tests/requirements.txt, or Isaac's bundled USD) is available.
-"""
+from conftest import register_unit_tests, repo_path
 
-from conftest import reexport_unit_tests, repo_path
-
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_interface_authoring.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_config.py b/tests/sim/optitrack_natnet_emulator/test_interface_config.py
index 5e2b139b6..a1e894bdc 100644
--- a/tests/sim/optitrack_natnet_emulator/test_interface_config.py
+++ b/tests/sim/optitrack_natnet_emulator/test_interface_config.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator interface config-model unit tests."""
+"""Proxy: registers optitrack.natnet.emulator interface config-model unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_interface_config.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
index 0ad8dc5e6..241b58e83 100644
--- a/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
+++ b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator pose-sampling / resync unit tests."""
+"""Proxy: registers optitrack.natnet.emulator pose-sampling / resync unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_pose_sampling.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
index f33b6d251..1d5480bf7 100644
--- a/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
+++ b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator end-to-end pose-streaming test."""
+"""Proxy: registers optitrack.natnet.emulator end-to-end pose-streaming test."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_pose_streaming.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_scene_setup.py b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py
index 4666c644a..8f124ee4d 100644
--- a/tests/sim/optitrack_natnet_emulator/test_scene_setup.py
+++ b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator launch-helper (scene_setup) tests."""
+"""Proxy: registers optitrack.natnet.emulator launch-helper (scene_setup) tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_scene_setup.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_serializers.py b/tests/sim/optitrack_natnet_emulator/test_serializers.py
index 22226cfff..f64cf3b35 100644
--- a/tests/sim/optitrack_natnet_emulator/test_serializers.py
+++ b/tests/sim/optitrack_natnet_emulator/test_serializers.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator serializer unit tests for pytest tests/."""
+"""Proxy: registers optitrack.natnet.emulator serializer unit tests for pytest tests/."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_serializers.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_server_from_config.py b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py
index f84c2e2a0..69cf75d96 100644
--- a/tests/sim/optitrack_natnet_emulator/test_server_from_config.py
+++ b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 server-from-config tests."""
+"""Proxy: registers NatNet emulator server-from-config unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_server_from_config.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
index 9cf4a45d6..f1eddeed9 100644
--- a/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
+++ b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator Commits 3-4 lifecycle unit tests."""
+"""Proxy: registers optitrack.natnet.emulator server lifecycle unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_server_lifecycle.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_target_resolution.py b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py
index 380b60e9b..803d21939 100644
--- a/tests/sim/optitrack_natnet_emulator/test_target_resolution.py
+++ b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 target-resolution tests."""
+"""Proxy: registers optitrack.natnet.emulator target-resolution unit tests."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_target_resolution.py",
diff --git a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
index e70a25064..b163407cc 100644
--- a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
+++ b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py
@@ -1,10 +1,10 @@
 # Copyright (c) 2024 Carnegie Mellon University
 # MIT License - see LICENSE in the repository root for full text.
-"""Proxy: re-exposes optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/."""
+"""Proxy: registers optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/."""
 
-from conftest import reexport_unit_tests, repo_path
+from conftest import register_unit_tests, repo_path
 
-reexport_unit_tests(
+register_unit_tests(
     globals(),
     repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"),
     "test_unicast_protocol.py",
diff --git a/tests/system/test_liveliness.py b/tests/system/test_liveliness.py
index b465726d5..60ebea446 100644
--- a/tests/system/test_liveliness.py
+++ b/tests/system/test_liveliness.py
@@ -33,17 +33,11 @@
     "/robot_{N}/trajectory_controller/trajectory_control_node",
 ]
 
-# Optional OptiTrack/NatNet liveliness sentinel — only meaningful when the robot
-# launches natnet_ros2 (LAUNCH_NATNET=true) and the sim streams NatNet frames
-# (Isaac emulator wrapper). Off by default, so normal runs skip it cleanly.
+# NatNet pose sentinel — only when LAUNCH_NATNET=true (robot natnet_ros2 + sim emulator).
 _NATNET_ENABLED = os.environ.get("LAUNCH_NATNET", "").strip().lower() in ("1", "true", "yes", "on")
 _NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
 _NATNET_MIN_HZ = 5.0
-# Generous first-message budget: on a cold Isaac boot the standalone Pegasus
-# script must finish loading, start Play (physics steps drive the emulator's
-# synchronous frame pump), and the robot natnet_ros2 client must connect over
-# UDP before pose frames appear. 60s was too tight vs the 600s /clock and 300s
-# sentinel-node budgets.
+# Cold Isaac boot: Pegasus load, Play, UDP connect — 60s was too tight vs other budgets.
 _NATNET_FIRST_MSG_TIMEOUT = 120
 
 
@@ -272,14 +266,11 @@ def ready():
     )
     @pytest.mark.dependency(depends=["sim_ready", "nodes"])
     def test_natnet_pose_alive(self, airstack_env):
-        """When NatNet is enabled, the OptiTrack pose topic must publish per robot.
-
-        Closes the loop the integration tier opens host-side: with the Isaac
-        emulator wrapper streaming in-sim and ``natnet_ros2`` running on each robot
-        (LAUNCH_NATNET=true), ``/{robot}/perception/optitrack/<body>/pose_cov`` must
-        be live. We probe ``/pose_cov`` (PoseWithCovarianceStamped) because it is
-        published unconditionally for every body, whereas the bare ``<body>``
-        PoseStamped is gated on the ``publish_direct_optitrack`` param.
+        """With LAUNCH_NATNET=true, /{robot}/perception/optitrack/{body}/pose_cov >= 5 Hz.
+
+        In-sim counterpart to the host-side integration tests. Uses pose_cov
+        (always published) rather than bare <body> (gated on
+        publish_direct_optitrack).
         """
         cfg = airstack_env["cfg"]
         robot_containers = get_robot_containers(airstack_env["robot_pattern"])

From 694720c49f5a3c8c7a6f08fdb7e22b4639347c5d Mon Sep 17 00:00:00 2001
From: John <johnliuchs2022@gmail.com>
Date: Wed, 17 Jun 2026 00:54:54 -0400
Subject: [PATCH 27/27] multiagent support, vision pose cleanup, example
 scripts cleanup

---
 .agents/skills/optitrack-development/SKILL.md |  11 +-
 .../isaac_sim/pegasus_scene_setup.md          |   2 +-
 docs/simulation/isaac_sim/spawning_drones.md  |   4 +-
 overrides/isaac-natnet-vision.env             |   2 +-
 .../src/perception/natnet_ros2/README.md      | 115 ++++----
 .../natnet_ros2/config/natnet_config.yaml     | 129 ++++++---
 .../include/natnet_ros2/natnet_logic.hpp      |  39 +++
 .../natnet_ros2/launch/natnet_ros2.launch.py  | 150 ++++++++--
 .../launch/vision_pose_converter.launch.xml   |  34 +--
 .../natnet_ros2/src/natnet_ros2_node.cpp      | 237 ++++++++--------
 .../src/vision_pose_converter_node.py         |  24 +-
 .../natnet_ros2/test/test_natnet_logic.cpp    |  40 +++
 .../natnet_ros2/test/test_natnet_ros2.py      | 162 ++++++++++-
 .../launch/perception.launch.xml              |   3 +-
 .../docker/sitl-files/px4-vision.env          |   2 +-
 .../optitrack.natnet.emulator/README.md       |   9 +-
 .../docs/natnet_interface_prim_design.md      |  47 ++--
 .../optitrack/natnet/emulator/defaults.py     |   2 +-
 .../natnet/emulator/isaac/__init__.py         |   8 +
 .../natnet/emulator/isaac/scene_setup.py      |  26 ++
 ...example_multi_px4_pegasus_launch_script.py |  46 ----
 ..._multi_px4_pegasus_natnet_launch_script.py | 256 ++++++++++++++++++
 .../example_one_px4_pegasus_launch_script.py  |  37 ---
 ...le_one_px4_pegasus_natnet_launch_script.py | 250 +++++++++++++++++
 tests/integration/natnet/README.md            |  42 +--
 .../natnet/test_natnet_integration.py         | 139 ++++++++--
 tests/system/test_liveliness.py               |  13 +-
 27 files changed, 1407 insertions(+), 422 deletions(-)
 create mode 100644 simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py
 create mode 100644 simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py

diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md
index c13aba904..fcfe66e87 100644
--- a/.agents/skills/optitrack-development/SKILL.md
+++ b/.agents/skills/optitrack-development/SKILL.md
@@ -43,9 +43,16 @@ flowchart LR
 
 **Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`.
 
-**Enable in sim:** the same `LAUNCH_NATNET=true` (passed into the isaac-sim container via [`docker-compose.yaml`](../../../simulation/isaac-sim/docker/docker-compose.yaml)) makes the Pegasus example launch scripts author a `/World/NatNetInterface` prim with one rigid body per drone `base_link` and **auto-start the emulator on load** (no UI). Helper: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`). Body name: single = `Drone`; multi = `Drone<i>` (id `i`). Override the name prefix with `NATNET_BODY_NAME`.
+**Enable in sim:** set ``ISAAC_SIM_SCRIPT_NAME`` to a NatNet Pegasus launch script (no env gate in the script — NatNet always starts):
 
-**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511 — see [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
+| Script | Use |
+|--------|-----|
+| [`example_one_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py) | Single drone + static ``Target`` |
+| [`example_multi_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py) | ``NUM_ROBOTS`` drones + shared ``Target`` (pair with 3-profile ``natnet_config.yaml``) |
+
+Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone``; multi = ``Drone<i>`` (id ``i``); target = ``Target`` (id 100). Override names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``.
+
+**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511. The config is per-robot: each `robots[$ROBOT_NAME]` profile lists the bodies it tracks (each a `rigid_body_name` + `id` mapped to a relative `topic`, with `pose`/`pose_cov` toggles and per-body covariance) and an optional `vision_pose` block that drives the MAVROS bridge. See [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
 
 ## NatNet: Two UDP Channels
 
diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md
index 5d1dc6f17..8fe10efb1 100644
--- a/docs/simulation/isaac_sim/pegasus_scene_setup.md
+++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md
@@ -177,7 +177,7 @@ SITL_PARAM_PROFILE="default"
 SITL_PARAM_PROFILE="px4-vision"
 ```
 
-Bundles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `px4-vision` with robot-side `LAUNCH_NATNET=true` and `publish_to_mavros: true` in `natnet_config.yaml`.
+Bundles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `px4-vision` with robot-side `LAUNCH_NATNET=true` and `vision_pose.enabled: true` in the robot's `natnet_config.yaml` profile.
 
 Convenience bundle: `airstack up --env-file overrides/isaac-natnet-vision.env`.
 
diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md
index f5919ecec..e7baa6a17 100644
--- a/docs/simulation/isaac_sim/spawning_drones.md
+++ b/docs/simulation/isaac_sim/spawning_drones.md
@@ -5,8 +5,10 @@ The reference launch scripts under `simulation/isaac-sim/launch_scripts/` cover
 | Script | Purpose |
 |---|---|
 | `barebones_pegasus_launch.py` | Minimal Pegasus boilerplate. Single drone, default environment, no scene import. Use as a template for new launch scripts. |
-| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, optional Ouster lidar) in the default environment. |
+| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, RTX lidar) in the default environment. |
 | `example_multi_px4_pegasus_launch_script.py` | `NUM_ROBOTS` drones spawned in a row in the default environment. Each drone gets its own ROS domain id (`1..N`). |
+| `example_one_px4_pegasus_natnet_launch_script.py` | Same as the one-drone script, plus OptiTrack NatNet streaming (drone + static target). Pair with `LAUNCH_NATNET=true` on the robot. |
+| `example_multi_px4_pegasus_natnet_launch_script.py` | Same as the multi-drone script, plus NatNet streaming (one body per drone + shared `Target`). Use with the 3-profile `natnet_config.yaml` scaffolding for shared-target demos. |
 | `example_multi_drone_scene_import.py` | `NUM_ROBOTS` drones in an **imported scene** (USD from a Nucleus server) with per-drone GPS homes set via `gps_utils.set_gps_origins`. Use this as the starting point for any custom scene. |
 
 The first three are vanilla Pegasus patterns; this page focuses on the multi-drone + custom-scene case where you also need correct GPS homes.
diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env
index 097128c60..8650e8926 100644
--- a/overrides/isaac-natnet-vision.env
+++ b/overrides/isaac-natnet-vision.env
@@ -4,4 +4,4 @@
 LAUNCH_NATNET=true
 SITL_PARAM_PROFILE=px4-vision
 ISAAC_SIM_USE_STANDALONE=true
-ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py
+ISAAC_SIM_SCRIPT_NAME=example_one_px4_pegasus_natnet_launch_script.py
diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md
index c357d86be..c34202b18 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/README.md
+++ b/robot/ros_ws/src/perception/natnet_ros2/README.md
@@ -15,8 +15,9 @@ This module provides a bridge between OptiTrack Motive motion capture systems an
 - Receives **NatNet UDP packets** from an external Motive PC (configurable IP/port)
 - **Decodes motion capture frames** containing rigid body positions and orientations
 - **Publishes pose data** to the AirStack perception layer in standard ROS 2 formats
-- **Supports multi-robot** via ROBOT_NAME namespacing
-- **Optionally bridges** to MAVROS for PX4 external pose feedback
+- **Tracks multiple rigid bodies per robot** (e.g. a drone for state estimation plus a separate target), each mapped to its own topic
+- **Supports multi-robot** via per-robot profiles selected by `ROBOT_NAME`
+- **Optionally bridges** to MAVROS for PX4 external pose feedback (per-robot)
 - **Respects OptiTrack licensing** by keeping the NatNet SDK external (host-side download with explicit consent)
 
 ## Architecture
@@ -25,15 +26,16 @@ This module provides a bridge between OptiTrack Motive motion capture systems an
 Motive (External PC)
     ↓ NatNet UDP (port 1511)
     ↓
-NatNet ROS 2 Node
-    ├→ /robot_1/perception/optitrack/{body_name}           (PoseStamped, optional)
-    ├→ /robot_1/perception/optitrack/{body_name}/pose_cov  (PoseWithCovarianceStamped, always)
-    └→ (Optional, publish_to_mavros: true)
+NatNet ROS 2 Node  (loads the ROBOT_NAME profile from natnet_config.yaml)
+    │  per configured body (one or more):
+    ├→ /{ROBOT_NAME}/{topic}           (PoseStamped, when pose: true)
+    ├→ /{ROBOT_NAME}/{topic}/pose_cov  (PoseWithCovarianceStamped, when pose_cov: true)
+    └→ (Optional, vision_pose.enabled: true)
          mavros_gp_origin_node
-           └→ /robot_1/interface/mavros/global_position/set_gp_origin
-         vision_pose_converter_node
-           ├→ /robot_1/interface/mavros/vision_pose/pose
-           └→ /robot_1/interface/mavros/vision_pose/pose_cov
+           └→ /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin
+         vision_pose_converter_node   (reads input/output topics from the profile)
+           ├→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose
+           └→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov
 ```
 
 ## Interfaces
@@ -41,37 +43,41 @@ NatNet ROS 2 Node
 ### Inputs
 
 - **Network**: NatNet UDP stream from Motive PC (external network)
-- **Configuration**: `natnet_config.yaml` with server IP, ports, `body_name`, and covariance
+- **Configuration**: `natnet_config.yaml` — generic `server` settings plus a `robots` map of per-robot profiles (body list + optional MAVROS `vision_pose` block). The launch file selects the profile matching `ROBOT_NAME`.
 
 ### Outputs
 
-For each tracked rigid body `{body_name}` from Motive:
+For each rigid body in the robot's profile, `topic` is a **relative** leaf namespaced
+under `/{ROBOT_NAME}/` (it defaults to `perception/optitrack/{rigid_body_name}` when
+omitted):
 
-#### Direct OptiTrack pose (optional)
+#### Direct OptiTrack pose
 
-- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}`
+- **Topic**: `/{ROBOT_NAME}/{topic}`
 - **Type**: `geometry_msgs/PoseStamped`
 - **Description**: Position and orientation only (no covariance)
-- **Enabled by**: `publish_direct_optitrack: true` in config (default: `true`)
+- **Enabled by**: `pose: true` on that body (per body)
 
-#### Pose with covariance (always)
+#### Pose with covariance
 
-- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov`
+- **Topic**: `/{ROBOT_NAME}/{topic}/pose_cov`
 - **Type**: `geometry_msgs/PoseWithCovarianceStamped`
-- **Description**: Same pose as above plus a 6×6 covariance matrix (`position_covariance` and `orientation_covariance` from config). Published whenever the rigid body is tracked — independent of `publish_direct_optitrack` and `publish_to_mavros`.
+- **Description**: Same pose plus a 6×6 covariance matrix from that body's `position_covariance` / `orientation_covariance`.
+- **Enabled by**: `pose_cov: true` on that body (per body)
 
-#### MAVROS vision pose bridge (optional)
+#### MAVROS vision pose bridge (optional, per robot)
 
-When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4:
+When the robot's `vision_pose.enabled: true`, `vision_pose_converter_node` subscribes to the configured `input_topic` (a body's `pose_cov`) and republishes for PX4 on the configured outputs:
 
-- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message)
-- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized)
-- **Enabled by**: `publish_to_mavros: true` in config
+- **Topic** (`output_pose_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message)
+- **Topic** (`output_pose_cov_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized)
+- **Enabled by**: `vision_pose.enabled: true` in the robot's profile
+- **Retargetable**: change `input_topic` / `output_pose_topic` / `output_pose_cov_topic` (relative, namespaced) to bridge to other middleware
 - **PX4 side**: set `SITL_PARAM_PROFILE=px4-vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/sitl-files/px4-vision.env`
 
 ##### Synthetic GPS origin (mocap / no-GNSS arming)
 
-With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `publish_to_mavros: true`,
+With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `vision_pose.enabled: true`,
 `mavros_gp_origin_node` publishes a synthetic origin once at startup:
 
 - **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped`
@@ -84,34 +90,50 @@ With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**
 
 ## Configuration
 
-Edit `config/natnet_config.yaml`:
+`config/natnet_config.yaml` uses a custom `natnet:` schema (not a flat ROS 2 param
+file): generic `server` settings shared by every agent, then a `robots` map of
+per-robot profiles. The launch file parses it, selects the profile matching the
+container's `ROBOT_NAME`, flattens the body list into node parameters, and brings up
+the MAVROS bridge only when that robot's `vision_pose.enabled` is true.
 
 ```yaml
-/**:
-  ros__parameters:
-    server_ip: "192.168.1.100"     # IP of the Motive PC
+natnet:
+  server:                              # generic across all agents
+    server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)"
     client_ip: "0.0.0.0"
     command_port: 1510
     data_port: 1511
-    connection_type: "unicast"      # or "multicast"
-
-    body_name: "Drone"              # rigid body name in Motive (case-sensitive)
-    body_id: -1                     # -1 = publish all bodies in the frame
-
-    publish_direct_optitrack: true   # PoseStamped on …/optitrack/{body_name}
-    publish_to_mavros: false        # include vision_pose_converter → MAVROS
-
+    connection_type: "unicast"         # or "multicast"
+    multicast_address: "239.255.42.99"
     frame_id: "world"
-
-    position_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
-    orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+    debug: false
+  robots:
+    robot_1:
+      vision_pose:                     # per-robot MAVROS bridge (omit/false to skip)
+        enabled: true
+        input_topic:           "perception/optitrack/drone/pose_cov"
+        output_pose_topic:     "interface/mavros/vision_pose/pose"
+        output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov"
+      bodies:                          # one or more tracked rigid bodies
+        - rigid_body_name: "Drone"     # Motive name (case-sensitive)
+          id: 1                        # Motive streaming id
+          topic: "perception/optitrack/drone"  # relative → /{ROBOT_NAME}/<topic>
+          pose: true                   # publish PoseStamped
+          pose_cov: true               # publish PoseWithCovarianceStamped
+          position_covariance:    [1.0e-6, 0, 0, 0, 1.0e-6, 0, 0, 0, 1.0e-6]
+          orientation_covariance: [3.0e-6, 0, 0, 0, 3.0e-6, 0, 0, 0, 3.0e-6]
 ```
 
+To track an additional body (e.g. a target) for a robot, add another entry under that
+robot's `bodies`. To add a robot, add a new key under `robots`. The shipped file
+includes commented scaffolding for a 3-drone fleet where `robot_1` and `robot_2` also
+track a shared `Target` body and `robot_3` tracks only its drone.
+
 ## Launch
 
 ### Basic launch
 
-Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides:
+Parameters come from `config/natnet_config.yaml` (server + the `ROBOT_NAME` profile). Optional overrides:
 
 ```bash
 ros2 launch natnet_ros2 natnet_ros2.launch.py \
@@ -122,7 +144,7 @@ ros2 launch natnet_ros2 natnet_ros2.launch.py \
 
 ### MAVROS bridge
 
-Set `publish_to_mavros: true` in `natnet_config.yaml`. The launch file reads `publish_to_mavros` and `body_name` from that YAML to decide whether to include `vision_pose_converter.launch.xml`.
+Set `vision_pose.enabled: true` in the robot's profile. The launch file includes `vision_pose_converter.launch.xml` (and `mavros_gp_origin.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`.
 
 ### From perception bringup
 
@@ -156,8 +178,9 @@ The SDK will be installed into `robot/ros_ws/src/perception/natnet_ros2/lib/` an
 
 ### Multi-Robot Support
 Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`:
-- Topics: `/{ROBOT_NAME}/perception/optitrack/{body_name}` and `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov`
-- Supported via launch file argument forwarding
+- The node loads the `robots[$ROBOT_NAME]` profile, so each robot tracks only the bodies (and runs the MAVROS bridge) configured for it.
+- Topics are namespaced under `/{ROBOT_NAME}/` from each body's relative `topic`.
+- Set `NUM_ROBOTS=N`; each replica resolves its own `ROBOT_NAME` (via `resolve_robot_name.py`) and auto-selects its profile — no per-robot env overrides.
 
 ### Error Handling
 - Invalid/malformed packets are skipped with debug logging
@@ -177,9 +200,9 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`:
    ```bash
    ros2 launch natnet_ros2 natnet_ros2.launch.py
    ```
-4. Verify topics:
+4. Verify topics (default profile maps the `Drone` body to `perception/optitrack/drone`):
    ```bash
-   ros2 topic echo /robot_1/perception/optitrack/Drone/pose_cov
+   ros2 topic echo /robot_1/perception/optitrack/drone/pose_cov
    ```
 
 ### Without Real Hardware (Mock)
@@ -187,7 +210,7 @@ TODO: Implement Motive simulator in Isaac Sim to generate fake NatNet packets
 
 ## Known Limitations
 
-- When `body_id: -1`, all rigid bodies in the Motive frame get publishers; filter by subscribing to the `{body_name}` you care about
+- The node publishes only bodies listed in the robot's profile (matched by `id`); bodies streamed by Motive but absent from the profile are ignored.
 - MAVROS bridge applies frame_id override and quaternion canonicalization; full PX4 frame alignment may still need tuning per airframe
 - No support for skeleton tracking or labeled markers yet (future enhancement)
 
diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
index 1e0ad6916..09d2ac3e5 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
+++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
@@ -1,51 +1,108 @@
-# NatNet ROS 2 parameters — loaded by natnet_ros2.launch.py (NatNet node + MAVROS gate).
-# publish_to_mavros / body_name are read by the launch file to decide vision_pose_converter include.
+# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py.
 #
-# Use /** so parameters apply regardless of namespace (e.g. /robot_1/perception/natnet_ros2_node).
-# See: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters.html
+# Unlike a plain ROS 2 parameter file, this uses a custom `natnet:` schema so a
+# single file can describe multiple robots, each tracking multiple rigid bodies.
+# The launch file selects the profile matching the container's ROBOT_NAME, flattens
+# it into node parameters, and (optionally) brings up the MAVROS vision_pose bridge.
+#
+# Schema:
+#   natnet:
+#     server:   generic connection settings shared by every agent
+#     robots:   one profile per ROBOT_NAME
+#       <robot_name>:
+#         vision_pose:  whether/how to forward a body to MAVROS (optional)
+#         bodies:       rigid bodies this robot subscribes to (one or more)
+
+natnet:
 
-/**:
-  ros__parameters:
-    # IP address of the PC running Motive (OptiTrack server).
-    # Defaults to the Isaac Sim container on the airstack_network (172.31.0.200),
+  # --- Connection settings (generic across all agents) -----------------------
+  server:
+    # IP of the PC running Motive (OptiTrack server). Defaults to the Isaac Sim
+    # container on the airstack_network (172.31.0.200).
     server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)"
-    # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have
-    # multiple NICs (e.g. Docker 172.17.* vs LAN).
+    # Motive learns the unicast destination from the outbound UDP source IP — bind
+    # explicitly when the client has multiple NICs (e.g. Docker 172.17.* vs LAN).
     client_ip: "0.0.0.0"
 
     command_port: 1510
     data_port: 1511
 
-    # "unicast"   — point-to-point; Motive streams directly to this machine's IP.
-    #               Requires Motive unicast streaming enabled and client_ip set
-    #               to the correct NIC when multiple interfaces are present.
-    # "multicast" — Motive broadcasts to a multicast group; any machine on the
-    #               subnet that joins the group receives all body data.
-    #               Use for multi-robot setups where every robot receives the
-    #               full frame and filters by body_id.
+    # "unicast"   — point-to-point; Motive streams directly to this machine (default).
+    # "multicast" — Motive broadcasts to a group; every robot receives the full frame
+    #               and filters by the body ids in its profile. Use for multi-robot.
     connection_type: "unicast"
-
-    # Only used when connection_type = "multicast".
-    # Must match Motive > Edit > Preferences > Data Streaming > Multicast Interface.
-    # OptiTrack default is 239.255.42.99.
+    # Only used when connection_type = "multicast" (OptiTrack default 239.255.42.99).
     multicast_address: "239.255.42.99"
 
-    # Name of the rigid body as defined in Motive. Must match exactly (case-sensitive).
-    body_name: "Drone"
-    body_id: -1
-
-    publish_direct_optitrack: true
-    publish_to_mavros: true
-
+    # Frame applied to every published pose. debug enables per-frame logging.
     frame_id: "world"
     debug: false
 
-    position_covariance:
-      [1.0e-6, 0.0, 0.0, # Covariances for sub 0.1 mm precision from optitrack
-       0.0, 1.0e-6, 0.0,
-       0.0, 0.0, 1.0e-6]
+  # --- Per-robot profiles (selected by ROBOT_NAME) ---------------------------
+  robots:
+
+    robot_1:
+      # MAVROS vision_pose bridge. enabled=false skips the converter entirely.
+      # input_topic is one of this robot's body pose_cov topics; the two outputs map
+      # to MAVROS's vision_pose/pose (PoseStamped) and vision_pose/pose_cov
+      # (PoseWithCovarianceStamped) subscribers. Topics are relative and namespaced
+      # under /{ROBOT_NAME}/ — redirect them to interface with other middleware.
+      vision_pose:
+        enabled: true
+        input_topic:           "perception/optitrack/drone/pose_cov"
+        output_pose_topic:     "interface/mavros/vision_pose/pose"
+        output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov"
+
+      # Rigid bodies this robot tracks. rigid_body_name must match Motive exactly
+      # (case-sensitive). topic is a relative leaf namespaced under /{ROBOT_NAME}/;
+      # pose / pose_cov toggle the PoseStamped and PoseWithCovarianceStamped variants.
+      bodies:
+        - rigid_body_name: "Drone"
+          id: 1
+          topic: "perception/optitrack/drone"
+          pose: true
+          pose_cov: true
+          # Covariances are per-body. Sub-0.1 mm / sub-0.1 deg for OptiTrack precision.
+          position_covariance:
+            [1.0e-6, 0.0,    0.0,
+             0.0,    1.0e-6, 0.0,
+             0.0,    0.0,    1.0e-6]
+          orientation_covariance:
+            [3.0e-6, 0.0,    0.0,
+             0.0,    3.0e-6, 0.0,
+             0.0,    0.0,    3.0e-6]
 
-    orientation_covariance:
-      [3.0e-6, 0.0, 0.0, # Covariances for sub 0.1 degree precision from optitrack
-       0.0, 3.0e-6, 0.0,
-       0.0, 0.0, 3.0e-6]
+    # --- Scaffolding: a 3-drone fleet where robot_1 & robot_2 also track a shared
+    # "Target" body and robot_3 tracks only its drone. Uncomment + set NUM_ROBOTS=3
+    # (and stream the matching bodies from example_multi_px4_pegasus_natnet_launch_script.py).
+    #
+    # robot_2:
+    #   vision_pose:
+    #     enabled: true
+    #     input_topic:           "perception/optitrack/drone/pose_cov"
+    #     output_pose_topic:     "interface/mavros/vision_pose/pose"
+    #     output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov"
+    #   bodies:
+    #     - rigid_body_name: "Drone2"
+    #       id: 2
+    #       topic: "perception/optitrack/drone"
+    #       pose: true
+    #       pose_cov: true
+    #     - rigid_body_name: "Target"       # shared target — also tracked by robot_1
+    #       id: 100
+    #       topic: "perception/optitrack/target"
+    #       pose: true
+    #       pose_cov: false
+    #
+    # robot_3:
+    #   vision_pose:
+    #     enabled: true
+    #     input_topic:           "perception/optitrack/drone/pose_cov"
+    #     output_pose_topic:     "interface/mavros/vision_pose/pose"
+    #     output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov"
+    #   bodies:
+    #     - rigid_body_name: "Drone3"
+    #       id: 3
+    #       topic: "perception/optitrack/drone"
+    #       pose: true
+    #       pose_cov: true
diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp
index f23216565..a0cf8927f 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp
+++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp
@@ -34,6 +34,7 @@
 
 #pragma once
 
+#include <algorithm>
 #include <array>
 #include <cstdint>
 #include <functional>
@@ -91,6 +92,34 @@ inline std::string optitrack_pose_cov_topic(
     return optitrack_topic_base(robot_name, body_name) + "/pose_cov";
 }
 
+/// Namespace a relative topic leaf under /{robot_name}/.
+///
+/// Leading slashes in \p relative are stripped so the result always has exactly
+/// one. Used for the per-body ``topic`` overrides in natnet_config.yaml, which are
+/// relative and namespaced by the node at runtime.
+inline std::string namespaced_topic(
+    const std::string & robot_name,
+    const std::string & relative)
+{
+    const std::size_t start = relative.find_first_not_of('/');
+    const std::string leaf =
+        (start == std::string::npos) ? std::string{} : relative.substr(start);
+    return "/" + robot_name + "/" + leaf;
+}
+
+/// Topic base for one configured body: the per-body relative override when set,
+/// otherwise the default /{robot_name}/perception/optitrack/{body_name}.
+inline std::string body_topic_base(
+    const std::string & robot_name,
+    const std::string & body_name,
+    const std::string & relative_override)
+{
+    if (relative_override.empty()) {
+        return optitrack_topic_base(robot_name, body_name);
+    }
+    return namespaced_topic(robot_name, relative_override);
+}
+
 
 // ===========================================================================
 // 3. Connection-configuration helpers
@@ -199,6 +228,16 @@ inline bool should_publish_body(int32_t filter_id, int32_t rb_id)
     return filter_id < 0 || rb_id == filter_id;
 }
 
+/// Returns true when rb_id is one of the configured body ids.
+///
+/// Multi-body variant of should_publish_body(): the node tracks a fixed set of
+/// ids from natnet_config.yaml and publishes only those (empty set → nothing).
+inline bool body_is_configured(const std::vector<int32_t> & configured_ids, int32_t rb_id)
+{
+    return std::find(configured_ids.begin(), configured_ids.end(), rb_id)
+           != configured_ids.end();
+}
+
 /// Double-precision pose extracted from a RigidBodySample.
 struct PoseData
 {
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
index 98a8b237c..6e49fcf88 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py
@@ -1,5 +1,10 @@
 #!/usr/bin/env python3
-"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml.
+"""Bring up the NatNet node from natnet_config.yaml; optionally the MAVROS bridge.
+
+The config uses a custom ``natnet:`` schema (server settings + per-robot profiles),
+so this launch file parses it, selects the profile matching ``ROBOT_NAME``, flattens
+the body list into node parameters, and — when the robot's ``vision_pose`` block is
+enabled — includes the MAVROS GP-origin + vision_pose_converter bridges.
 
 natnet_ros2_node is a C++ executable that requires the OptiTrack NatNet SDK.
 If the SDK was not installed (``airstack setup`` not run) and the workspace
@@ -10,8 +15,9 @@
 from __future__ import annotations
 
 import os
+import re
 from pathlib import Path
-from typing import cast
+from typing import Any, cast
 
 import yaml
 from ament_index_python.packages import get_package_share_directory
@@ -20,11 +26,28 @@
 from launch.launch_description_sources import FrontendLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterFile
+
+# Per-body covariance fallback when a body omits its own (sub-0.1 mm / sub-0.1 deg).
+_DEFAULT_POSITION_COVARIANCE = [1.0e-6, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 1.0e-6]
+_DEFAULT_ORIENTATION_COVARIANCE = [3.0e-6, 0.0, 0.0, 0.0, 3.0e-6, 0.0, 0.0, 0.0, 3.0e-6]
+
+_ENV_SUBST = re.compile(r"\$\(env\s+(\w+)(?:\s+([^)]*))?\)")
+
+
+def _expand_env(value: Any) -> Any:
+    """Expand ``$(env VAR default)`` tokens in a string using os.environ."""
+    if not isinstance(value, str):
+        return value
+
+    def _replace(match: re.Match) -> str:
+        var, default = match.group(1), match.group(2)
+        return os.environ.get(var, default if default is not None else "")
+
+    return _ENV_SUBST.sub(_replace, value)
 
 
-def _ros_params_from_file(config_path: str) -> dict:
-    """Parse /** / ros__parameters block from a ROS 2 parameter YAML."""
+def _load_natnet_config(config_path: str) -> dict:
+    """Parse the ``natnet:`` block from the config YAML."""
     path = Path(config_path)
     if not path.is_file():
         return {}
@@ -32,11 +55,70 @@ def _ros_params_from_file(config_path: str) -> dict:
         data = yaml.safe_load(f)
     if not isinstance(data, dict):
         return {}
-    block = data.get('/**')
-    if not isinstance(block, dict):
-        return {}
-    params = block.get('ros__parameters', {})
-    return cast(dict, params) if isinstance(params, dict) else {}
+    natnet = data.get('natnet', {})
+    return cast(dict, natnet) if isinstance(natnet, dict) else {}
+
+
+def _flatten_covariance(values: Any, fallback: list[float]) -> list[float]:
+    """Coerce a 9-element covariance block to floats, falling back when absent."""
+    if not isinstance(values, (list, tuple)) or len(values) == 0:
+        return list(fallback)
+    return [float(v) for v in values]
+
+
+def _build_node_params(server: dict, profile: dict) -> dict:
+    """Flatten the server block + a robot's body list into node parameters."""
+    bodies = profile.get('bodies', []) or []
+
+    params: dict[str, Any] = {
+        'server_ip': str(_expand_env(server.get('server_ip', '172.31.0.200'))),
+        'client_ip': str(_expand_env(server.get('client_ip', '0.0.0.0'))),
+        'command_port': int(server.get('command_port', 1510)),
+        'data_port': int(server.get('data_port', 1511)),
+        'connection_type': str(server.get('connection_type', 'unicast')),
+        'multicast_address': str(server.get('multicast_address', '239.255.42.99')),
+        'frame_id': str(server.get('frame_id', 'world')),
+        'debug': bool(server.get('debug', False)),
+    }
+
+    body_names: list[str] = []
+    body_ids: list[int] = []
+    body_topics: list[str] = []
+    body_pose: list[bool] = []
+    body_pose_cov: list[bool] = []
+    body_position_covariance: list[float] = []
+    body_orientation_covariance: list[float] = []
+
+    for body in bodies:
+        body_names.append(str(body.get('rigid_body_name', '')))
+        body_ids.append(int(body.get('id', -1)))
+        body_topics.append(str(body.get('topic', '')))
+        body_pose.append(bool(body.get('pose', True)))
+        body_pose_cov.append(bool(body.get('pose_cov', True)))
+        body_position_covariance.extend(
+            _flatten_covariance(body.get('position_covariance'), _DEFAULT_POSITION_COVARIANCE)
+        )
+        body_orientation_covariance.extend(
+            _flatten_covariance(body.get('orientation_covariance'), _DEFAULT_ORIENTATION_COVARIANCE)
+        )
+
+    params.update(
+        {
+            'body_names': body_names,
+            'body_ids': body_ids,
+            'body_topics': body_topics,
+            'body_pose': body_pose,
+            'body_pose_cov': body_pose_cov,
+            'body_position_covariance': body_position_covariance,
+            'body_orientation_covariance': body_orientation_covariance,
+        }
+    )
+    return params
+
+
+def _namespaced(robot_name: str, relative: str) -> str:
+    """Namespace a relative topic under /{robot_name}/."""
+    return '/' + robot_name + '/' + relative.lstrip('/')
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -56,13 +138,27 @@ def launch_setup(context, *_args, **_kwargs):
         gp_path = gp_origin_config_file.perform(context)
         ust = use_sim_time.perform(context)
 
-        ros_params = _ros_params_from_file(cfg_path)
-        publish_mavros = bool(ros_params.get('publish_to_mavros', False))
-        body_name = str(ros_params.get('body_name', 'robot_1'))
+        robot_name = os.environ.get('ROBOT_NAME', 'robot_1')
+        natnet = _load_natnet_config(cfg_path)
+        server = natnet.get('server', {}) if isinstance(natnet, dict) else {}
+        robots = natnet.get('robots', {}) if isinstance(natnet, dict) else {}
+        profile = robots.get(robot_name, {}) if isinstance(robots, dict) else {}
+
+        if not profile:
+            print(
+                f"[natnet_ros2.launch] WARNING: no profile for ROBOT_NAME='{robot_name}' "
+                f"in {cfg_path}; node will start with no tracked bodies."
+            )
+
+        node_params = _build_node_params(server, profile)
+        # launch_ros / rclpy cannot infer the type of an empty-list parameter, so drop
+        # any empty arrays; the node declares matching empty defaults and tracks nothing.
+        node_params = {
+            k: v for k, v in node_params.items() if not (isinstance(v, list) and len(v) == 0)
+        }
 
         # pkg_share = <prefix>/share/natnet_ros2 → go up two levels to reach <prefix>,
         # then down into lib/natnet_ros2/ where colcon installs executables.
-        pkg_share = get_package_share_directory('natnet_ros2')
         node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node'
         if not node_path.exists():
             raise RuntimeError(
@@ -78,11 +174,23 @@ def launch_setup(context, *_args, **_kwargs):
                 executable='natnet_ros2_node',
                 name='natnet_ros2_node',
                 output='screen',
-                parameters=[ParameterFile(config_file, allow_substs=True)],
+                parameters=[node_params],
             ),
         ]
 
-        if publish_mavros:
+        vision_pose = profile.get('vision_pose', {}) if isinstance(profile, dict) else {}
+        if vision_pose.get('enabled', False):
+            input_topic = _namespaced(
+                robot_name, str(vision_pose.get('input_topic', 'perception/optitrack/drone/pose_cov'))
+            )
+            output_pose_topic = _namespaced(
+                robot_name, str(vision_pose.get('output_pose_topic', 'interface/mavros/vision_pose/pose'))
+            )
+            output_pose_cov_topic = _namespaced(
+                robot_name,
+                str(vision_pose.get('output_pose_cov_topic', 'interface/mavros/vision_pose/pose_cov')),
+            )
+
             actions.append(
                 IncludeLaunchDescription(
                     FrontendLaunchDescriptionSource(
@@ -101,7 +209,9 @@ def launch_setup(context, *_args, **_kwargs):
                     ),
                     launch_arguments=[
                         ('config_file', vp_path),
-                        ('body_name', body_name),
+                        ('input_topic', input_topic),
+                        ('output_pose_topic', output_pose_topic),
+                        ('output_pose_cov_topic', output_pose_cov_topic),
                         ('use_sim_time', ust),
                     ],
                 ),
@@ -113,13 +223,13 @@ def launch_setup(context, *_args, **_kwargs):
             DeclareLaunchArgument(
                 'config_file',
                 default_value=default_natnet_yaml,
-                description='NatNet parameter YAML (/** ros__parameters). '
-                'publish_to_mavros and body_name control MAVROS include.',
+                description='NatNet config YAML (natnet: server + per-robot profiles). '
+                'The robot profile selected by ROBOT_NAME drives bodies + MAVROS include.',
             ),
             DeclareLaunchArgument(
                 'vision_pose_config_file',
                 default_value=default_vp_yaml,
-                description='vision_pose_converter parameter YAML.',
+                description='vision_pose_converter parameter YAML (frame_id, canonical_quaternion).',
             ),
             DeclareLaunchArgument(
                 'gp_origin_config_file',
diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
index 209b22059..803cbdad7 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
+++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml
@@ -3,26 +3,31 @@
   <!--
     Vision Pose Converter Launch File
 
-    Optional bridge that re-publishes NatNet pose_cov data on the MAVROS
+    Optional bridge that re-publishes a NatNet pose_cov topic on the MAVROS
     vision_pose topics so PX4 can fuse external pose estimates.
 
     Static params (frame_id, child_frame_id, default use_sim_time): config/vision_pose_converter.yaml
-    Topic wiring still uses body_name + ROBOT_NAME below.
+    Topics are supplied by natnet_ros2.launch.py from the robot's vision_pose block
+    in natnet_config.yaml (absolute, already namespaced under /{ROBOT_NAME}/):
 
-    Input  (from NatNet node):
-      /{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov
-
-    Output (consumed by MAVROS):
-      /{ROBOT_NAME}/interface/mavros/vision_pose/pose
-      /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov
+      input_topic            (from NatNet node, PoseWithCovarianceStamped)
+      output_pose_topic      (to MAVROS, PoseStamped)
+      output_pose_cov_topic  (to MAVROS, PoseWithCovarianceStamped)
   -->
 
   <arg name="config_file"
        default="$(find-pkg-share natnet_ros2)/config/vision_pose_converter.yaml"
        description="ROS 2 parameter YAML (/** ros__parameters block)"/>
 
-  <arg name="body_name" default="robot_1"
-       description="Body name matching NatNet topic suffix (must match natnet_config.yaml)"/>
+  <arg name="input_topic"
+       default="/$(env ROBOT_NAME robot_1)/perception/optitrack/drone/pose_cov"
+       description="NatNet pose_cov topic to bridge to MAVROS"/>
+  <arg name="output_pose_topic"
+       default="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose"
+       description="MAVROS vision_pose PoseStamped output topic"/>
+  <arg name="output_pose_cov_topic"
+       default="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose_cov"
+       description="MAVROS vision_pose PoseWithCovarianceStamped output topic"/>
 
   <!-- Overrides vision_pose_converter.yaml use_sim_time when launching from sim -->
   <arg name="use_sim_time" default="false"
@@ -32,12 +37,9 @@
         name="vision_pose_converter" output="screen">
     <param from="$(var config_file)" allow_substs="true"/>
 
-    <remap from="input_pose"
-           to="/$(env ROBOT_NAME robot_1)/perception/optitrack/$(var body_name)/pose_cov"/>
-    <remap from="output_pose"
-           to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose"/>
-    <remap from="output_pose_cov"
-           to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose_cov"/>
+    <param name="input_topic" value="$(var input_topic)"/>
+    <param name="output_pose_topic" value="$(var output_pose_topic)"/>
+    <param name="output_pose_cov_topic" value="$(var output_pose_cov_topic)"/>
 
     <param name="use_sim_time" value="$(var use_sim_time)"/>
   </node>
diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
index c93890e0d..d754a1cef 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
+++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp
@@ -2,25 +2,27 @@
 //
 // ROS 2 NatNet SDK node for OptiTrack Motive integration.
 //
-// Published topics (per tracked rigid body):
-//   /{robot_name}/perception/optitrack/{body_name}           → PoseStamped
-//   /{robot_name}/perception/optitrack/{body_name}/pose_cov  → PoseWithCovarianceStamped
+// Published topics (per configured rigid body):
+//   /{robot_name}/{topic}            → PoseStamped               (when pose=true)
+//   /{robot_name}/{topic}/pose_cov   → PoseWithCovarianceStamped (when pose_cov=true)
+// where {topic} defaults to perception/optitrack/{rigid_body_name} when unset.
 //
-// Parameters (see config/natnet_config.yaml):
-//   server_ip, client_ip, command_port, data_port,
-//   body_name, body_id (-1 = all), publish_direct_optitrack,
-//   frame_id, debug, position_covariance, orientation_covariance
+// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py:
+//   server_ip, client_ip, command_port, data_port, connection_type,
+//   multicast_address, frame_id, debug, and parallel per-body arrays:
+//   body_names[], body_ids[], body_topics[], body_pose[], body_pose_cov[],
+//   body_position_covariance[] / body_orientation_covariance[] (9·N, sliced per body).
 //
 // ROBOT_NAME is read from the environment variable set by AirStack's
 // robot_name_map resolver at container startup.
 
+#include <algorithm>
 #include <array>
-#include <atomic>
 #include <cstdlib>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <unordered_map>
+#include <vector>
 
 // ROS 2
 #include "rclcpp/rclcpp.hpp"
@@ -41,8 +43,14 @@ class NatNetROS2Node : public rclcpp::Node
     using PoseStamped               = geometry_msgs::msg::PoseStamped;
     using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 
-    struct BodyPublishers
+    struct BodyConfig
     {
+        int32_t                                                id = -1;
+        std::string                                            rigid_body_name;
+        std::string                                            topic_base;
+        bool                                                   publish_pose     = true;
+        bool                                                   publish_pose_cov = true;
+        std::array<double, 36>                                 covariance{};
         rclcpp::Publisher<PoseStamped>::SharedPtr               pose_pub;
         rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pose_cov_pub;
     };
@@ -52,24 +60,23 @@ class NatNetROS2Node : public rclcpp::Node
     : Node("natnet_ros2_node")
     {
         // ----- Parameters --------------------------------------------------
-        this->declare_parameter("server_ip",                "192.168.1.1");
-        this->declare_parameter("client_ip",                "0.0.0.0");
-        this->declare_parameter("command_port",             1510);
-        this->declare_parameter("data_port",                1511);
-        this->declare_parameter("connection_type",          std::string("unicast"));
-        this->declare_parameter("multicast_address",        std::string("239.255.42.99"));
-        this->declare_parameter("body_name",                "robot_1");
-        this->declare_parameter("body_id",                  -1);
-        this->declare_parameter("publish_direct_optitrack", true);
-        this->declare_parameter("publish_to_mavros",        false);
-        this->declare_parameter("frame_id",                 "world");
-        this->declare_parameter("debug",                    false);
-        this->declare_parameter(
-            "position_covariance",
-            std::vector<double>{0.1,0.,0., 0.,0.1,0., 0.,0.,0.1});
-        this->declare_parameter(
-            "orientation_covariance",
-            std::vector<double>{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01});
+        this->declare_parameter("server_ip",         "192.168.1.1");
+        this->declare_parameter("client_ip",         "0.0.0.0");
+        this->declare_parameter("command_port",      1510);
+        this->declare_parameter("data_port",         1511);
+        this->declare_parameter("connection_type",   std::string("unicast"));
+        this->declare_parameter("multicast_address", std::string("239.255.42.99"));
+        this->declare_parameter("frame_id",          "world");
+        this->declare_parameter("debug",             false);
+
+        // Parallel per-body arrays (flattened from natnet_config.yaml by the launch file).
+        this->declare_parameter("body_names",   std::vector<std::string>{});
+        this->declare_parameter("body_ids",     std::vector<int64_t>{});
+        this->declare_parameter("body_topics",  std::vector<std::string>{});
+        this->declare_parameter("body_pose",    std::vector<bool>{});
+        this->declare_parameter("body_pose_cov", std::vector<bool>{});
+        this->declare_parameter("body_position_covariance",    std::vector<double>{});
+        this->declare_parameter("body_orientation_covariance", std::vector<double>{});
 
         // ----- Read parameters ---------------------------------------------
         const auto connect_cfg = natnet_ros2::make_connect_config(
@@ -88,19 +95,14 @@ class NatNetROS2Node : public rclcpp::Node
                 this->get_parameter("connection_type").as_string().c_str());
         }
 
-        body_name_      = this->get_parameter("body_name").as_string();
-        body_id_        = static_cast<int32_t>(this->get_parameter("body_id").as_int());
-        publish_direct_ = this->get_parameter("publish_direct_optitrack").as_bool();
-        frame_id_       = this->get_parameter("frame_id").as_string();
-        debug_          = this->get_parameter("debug").as_bool();
-
-        covariance_6x6_ = natnet_ros2::build_covariance_6x6(
-            this->get_parameter("position_covariance").as_double_array(),
-            this->get_parameter("orientation_covariance").as_double_array());
+        frame_id_ = this->get_parameter("frame_id").as_string();
+        debug_    = this->get_parameter("debug").as_bool();
 
         const char * rn = std::getenv("ROBOT_NAME");
         robot_name_ = rn ? rn : "robot_1";
 
+        build_body_configs();
+
         RCLCPP_INFO(get_logger(), "=========================================");
         RCLCPP_INFO(get_logger(), "NatNet ROS 2 Node");
         RCLCPP_INFO(get_logger(), "  robot_name:      %s", robot_name_.c_str());
@@ -110,9 +112,7 @@ class NatNetROS2Node : public rclcpp::Node
         if (natnet_ros2::is_multicast(connect_cfg)) {
             RCLCPP_INFO(get_logger(), "  multicast_addr:  %s", connect_cfg.multicast_address.c_str());
         }
-        RCLCPP_INFO(get_logger(), "  body_id:         %d (%s)",
-            static_cast<int>(body_id_),
-            (body_id_ < 0) ? "track all" : "single body");
+        RCLCPP_INFO(get_logger(), "  tracked bodies:  %zu", bodies_.size());
         RCLCPP_INFO(get_logger(), "=========================================");
 
         // Production client — NatNetClientAdapter wraps the SDK
@@ -125,10 +125,6 @@ class NatNetROS2Node : public rclcpp::Node
                 std::chrono::seconds(2),
                 std::bind(&NatNetROS2Node::retry_connect, this));
         }
-
-        refresh_timer_ = this->create_wall_timer(
-            std::chrono::seconds(1),
-            std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this));
     }
 
     // -----------------------------------------------------------------------
@@ -139,14 +135,10 @@ class NatNetROS2Node : public rclcpp::Node
 
     // -----------------------------------------------------------------------
     // Called from the NatNetClientAdapter's frame trampoline.
-    // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access.
+    // publish() and Clock::now() are thread-safe; bodies_ is immutable after init.
     // -----------------------------------------------------------------------
     void on_frame(const natnet_ros2::FrameSample & frame)
     {
-        if (natnet_ros2::model_list_changed(frame.params)) {
-            needs_description_refresh_.store(true, std::memory_order_relaxed);
-        }
-
         if (debug_) {
             RCLCPP_DEBUG(get_logger(), "Frame %d: %zu rigid bodies, ts=%.4f s",
                 frame.frame_num, frame.bodies.size(), static_cast<double>(frame.timestamp));
@@ -161,20 +153,14 @@ class NatNetROS2Node : public rclcpp::Node
                 }
                 continue;
             }
-            if (!natnet_ros2::should_publish_body(body_id_, rb.id)) { continue; }
-
-            std::lock_guard<std::mutex> lock(pub_mutex_);
 
-            const auto pub_it = publishers_.find(rb.id);
-            if (pub_it == publishers_.end()) {
-                needs_description_refresh_.store(true, std::memory_order_relaxed);
-                continue;
-            }
+            const auto it = bodies_.find(rb.id);
+            if (it == bodies_.end()) { continue; }  // not configured for this robot
 
             const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose(rb);
-            const BodyPublishers & bp = pub_it->second;
+            const BodyConfig & body = it->second;
 
-            if (publish_direct_ && bp.pose_pub) {
+            if (body.publish_pose && body.pose_pub) {
                 PoseStamped msg;
                 msg.header.frame_id    = frame_id_;
                 msg.header.stamp       = stamp;
@@ -185,10 +171,10 @@ class NatNetROS2Node : public rclcpp::Node
                 msg.pose.orientation.y = pose.qy;
                 msg.pose.orientation.z = pose.qz;
                 msg.pose.orientation.w = pose.qw;
-                bp.pose_pub->publish(msg);
+                body.pose_pub->publish(msg);
             }
 
-            if (bp.pose_cov_pub) {
+            if (body.publish_pose_cov && body.pose_cov_pub) {
                 PoseWithCovarianceStamped cov_msg;
                 cov_msg.header.frame_id         = frame_id_;
                 cov_msg.header.stamp            = stamp;
@@ -199,8 +185,8 @@ class NatNetROS2Node : public rclcpp::Node
                 cov_msg.pose.pose.orientation.y = pose.qy;
                 cov_msg.pose.pose.orientation.z = pose.qz;
                 cov_msg.pose.pose.orientation.w = pose.qw;
-                cov_msg.pose.covariance         = covariance_6x6_;
-                bp.pose_cov_pub->publish(cov_msg);
+                cov_msg.pose.covariance         = body.covariance;
+                body.pose_cov_pub->publish(cov_msg);
             }
         }
     }
@@ -224,8 +210,6 @@ class NatNetROS2Node : public rclcpp::Node
             RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str());
         }
 
-        refresh_descriptions_locked();
-
         client_->set_frame_callback(
             [this](const natnet_ros2::FrameSample & f) { on_frame(f); });
         RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data.");
@@ -250,98 +234,95 @@ class NatNetROS2Node : public rclcpp::Node
     }
 
     // -----------------------------------------------------------------------
-    void refresh_descriptions_if_needed()
-    {
-        if (!needs_description_refresh_.exchange(false, std::memory_order_relaxed)) {
-            return;
-        }
-        RCLCPP_INFO(get_logger(), "Model list change detected — refreshing data descriptions.");
-        std::lock_guard<std::mutex> lock(pub_mutex_);
-        refresh_descriptions_locked();
-    }
-
-    // -----------------------------------------------------------------------
-    // Must be called with pub_mutex_ held (or from single-threaded init).
+    // Build the per-body config map + publishers from the parallel param arrays.
+    // Publishers are created up front (config-driven), so streaming begins as soon
+    // as frames arrive — no dependency on Motive's data-description handshake.
     // -----------------------------------------------------------------------
-    void refresh_descriptions_locked()
+    void build_body_configs()
     {
-        if (!client_) { return; }
-
-        // Always ensure the statically-configured body has a publisher
-        if (body_id_ >= 0) {
-            ensure_publisher_locked(body_id_, body_name_);
+        const auto names    = this->get_parameter("body_names").as_string_array();
+        const auto ids      = this->get_parameter("body_ids").as_integer_array();
+        const auto topics   = this->get_parameter("body_topics").as_string_array();
+        const auto pose     = this->get_parameter("body_pose").as_bool_array();
+        const auto pose_cov = this->get_parameter("body_pose_cov").as_bool_array();
+        const auto pos_cov  = this->get_parameter("body_position_covariance").as_double_array();
+        const auto ori_cov  = this->get_parameter("body_orientation_covariance").as_double_array();
+
+        const std::size_t n = std::min(names.size(), ids.size());
+        if (names.size() != ids.size()) {
+            RCLCPP_WARN(get_logger(),
+                "body_names (%zu) and body_ids (%zu) length mismatch — using %zu.",
+                names.size(), ids.size(), n);
         }
 
-        const auto bodies = client_->get_body_descriptors();
-        int newly_created = 0;
-        for (const auto & bd : bodies) {
-            // Store name for every body (including skeleton bones)
-            body_names_[bd.id] = bd.name;
+        for (std::size_t i = 0; i < n; ++i) {
+            BodyConfig body;
+            body.id              = static_cast<int32_t>(ids[i]);
+            body.rigid_body_name = names[i];
+            body.publish_pose     = (i < pose.size())     ? pose[i]     : true;
+            body.publish_pose_cov = (i < pose_cov.size()) ? pose_cov[i] : true;
 
-            // Skip skeleton bones (parent_id >= 0)
-            if (bd.parent_id >= 0) { continue; }
+            const std::string relative = (i < topics.size()) ? topics[i] : std::string{};
+            body.topic_base =
+                natnet_ros2::body_topic_base(robot_name_, body.rigid_body_name, relative);
 
-            // When tracking a single body, skip others
-            if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; }
+            body.covariance = natnet_ros2::build_covariance_6x6(
+                cov_slice(pos_cov, i, _DEFAULT_POSITION_COVARIANCE),
+                cov_slice(ori_cov, i, _DEFAULT_ORIENTATION_COVARIANCE));
 
-            if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; }
-        }
+            if (body.publish_pose) {
+                body.pose_pub = this->create_publisher<PoseStamped>(body.topic_base, 10);
+            }
+            if (body.publish_pose_cov) {
+                body.pose_cov_pub = this->create_publisher<PoseWithCovarianceStamped>(
+                    body.topic_base + "/pose_cov", 10);
+            }
 
-        if (newly_created > 0) {
             RCLCPP_INFO(get_logger(),
-                "Data descriptions refreshed: %d new publisher(s) created.", newly_created);
-        } else {
-            RCLCPP_DEBUG(get_logger(), "Data descriptions refreshed: no new publishers.");
+                "Tracking body id=%d name='%s' → %s (pose=%d pose_cov=%d)",
+                static_cast<int>(body.id), body.rigid_body_name.c_str(),
+                body.topic_base.c_str(),
+                static_cast<int>(body.publish_pose),
+                static_cast<int>(body.publish_pose_cov));
+
+            bodies_.emplace(body.id, std::move(body));
         }
     }
 
     // -----------------------------------------------------------------------
-    bool ensure_publisher_locked(int32_t id, const std::string & name)
+    // Return the i-th 9-element covariance block from a flattened array, or the
+    // built-in default when the slice is missing.
+    static std::vector<double> cov_slice(
+        const std::vector<double> & flat, std::size_t i, const std::vector<double> & fallback)
     {
-        if (publishers_.count(id)) { return false; }
-
-        const std::string topic_base =
-            natnet_ros2::optitrack_topic_base(robot_name_, name);
-
-        BodyPublishers bp;
-        if (publish_direct_) {
-            bp.pose_pub = this->create_publisher<PoseStamped>(topic_base, 10);
-        }
-        bp.pose_cov_pub = this->create_publisher<PoseWithCovarianceStamped>(
-            natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10);
-
-        publishers_.emplace(id, std::move(bp));
-
-        RCLCPP_INFO(get_logger(),
-            "Publisher registered: id=%d  name='%s'  → %s[/pose_cov]",
-            static_cast<int>(id), name.c_str(), topic_base.c_str());
-        return true;
+        const std::size_t start = i * 9;
+        if (flat.size() < start + 9) { return fallback; }
+        return std::vector<double>(flat.begin() + start, flat.begin() + start + 9);
     }
 
     // -----------------------------------------------------------------------
     // Parameters / state
-    std::string  body_name_;
-    int32_t      body_id_          = -1;
-    bool         publish_direct_   = true;
     std::string  frame_id_;
-    bool         debug_            = false;
+    bool         debug_     = false;
     std::string  robot_name_;
 
-    std::array<double, 36> covariance_6x6_{};
-
     std::unique_ptr<natnet_ros2::INatNetClient> client_;
     natnet_ros2::ConnectConfig connect_cfg_;
     bool connected_ = false;
 
-    std::mutex pub_mutex_;
-    std::unordered_map<int32_t, std::string>    body_names_;
-    std::unordered_map<int32_t, BodyPublishers> publishers_;
+    std::unordered_map<int32_t, BodyConfig> bodies_;
 
-    std::atomic<bool> needs_description_refresh_{false};
-    rclcpp::TimerBase::SharedPtr refresh_timer_;
     rclcpp::TimerBase::SharedPtr connect_timer_;
+
+    static const std::vector<double> _DEFAULT_POSITION_COVARIANCE;
+    static const std::vector<double> _DEFAULT_ORIENTATION_COVARIANCE;
 };
 
+const std::vector<double> NatNetROS2Node::_DEFAULT_POSITION_COVARIANCE =
+    {1.0e-6, 0., 0., 0., 1.0e-6, 0., 0., 0., 1.0e-6};
+const std::vector<double> NatNetROS2Node::_DEFAULT_ORIENTATION_COVARIANCE =
+    {3.0e-6, 0., 0., 0., 3.0e-6, 0., 0., 0., 3.0e-6};
+
 
 // ---------------------------------------------------------------------------
 int main(int argc, char ** argv)
diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py
index 9a36f879d..81d2df3dd 100755
--- a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py
+++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py
@@ -7,6 +7,12 @@
 for PX4 external pose estimation and state fusion.
 
 Converts from NatNet coordinate frame to a frame suitable for MAVROS.
+
+Topics are configurable so the bridge can be retargeted to other middleware.
+``input_topic`` / ``output_pose_topic`` / ``output_pose_cov_topic`` default to the
+relative names ``input_pose`` / ``output_pose`` / ``output_pose_cov`` (remappable),
+but natnet_ros2.launch.py overrides them with the absolute, ROBOT_NAME-namespaced
+topics from the robot's ``vision_pose`` block in natnet_config.yaml.
 """
 
 import rclpy
@@ -28,15 +34,23 @@ def __init__(self):
         self.declare_parameter('frame_id', 'world')
         self.declare_parameter('child_frame_id', 'base_link')
         self.declare_parameter('canonical_quaternion', True)
+        # Topic names — overridden by the launch file from the per-robot
+        # vision_pose block; defaults are the historical remappable relative names.
+        self.declare_parameter('input_topic', 'input_pose')
+        self.declare_parameter('output_pose_topic', 'output_pose')
+        self.declare_parameter('output_pose_cov_topic', 'output_pose_cov')
 
         self.frame_id = self.get_parameter('frame_id').value
         self.child_frame_id = self.get_parameter('child_frame_id').value
         self.canonical_quaternion = self.get_parameter('canonical_quaternion').value
+        input_topic = self.get_parameter('input_topic').value
+        output_pose_topic = self.get_parameter('output_pose_topic').value
+        output_pose_cov_topic = self.get_parameter('output_pose_cov_topic').value
 
         # Subscribers
         self.pose_sub = self.create_subscription(
             PoseWithCovarianceStamped,
-            'input_pose',
+            input_topic,
             self._on_pose,
             10
         )
@@ -44,19 +58,21 @@ def __init__(self):
         # Publishers
         self.pose_pub = self.create_publisher(
             PoseStamped,
-            'output_pose',
+            output_pose_topic,
             10
         )
         self.pose_cov_pub = self.create_publisher(
             PoseWithCovarianceStamped,
-            'output_pose_cov',
+            output_pose_cov_topic,
             10
         )
 
         self.get_logger().info(
             f'Vision pose converter started '
             f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r}, '
-            f'canonical_quaternion={self.canonical_quaternion})'
+            f'canonical_quaternion={self.canonical_quaternion}, '
+            f'input_topic={input_topic!r}, output_pose_topic={output_pose_topic!r}, '
+            f'output_pose_cov_topic={output_pose_cov_topic!r})'
         )
 
     @staticmethod
diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp
index 7a144ef9b..5796c121b 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp
+++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp
@@ -136,6 +136,46 @@ TEST(TopicNames, LeadingSlashPresent)
     EXPECT_EQ(optitrack_topic_base("robot_1", "Body")[0], '/');
 }
 
+TEST(TopicNames, NamespacedTopicStripsLeadingSlashes)
+{
+    EXPECT_EQ(namespaced_topic("robot_1", "perception/optitrack/drone"),
+              "/robot_1/perception/optitrack/drone");
+    EXPECT_EQ(namespaced_topic("robot_1", "/perception/optitrack/drone"),
+              "/robot_1/perception/optitrack/drone");
+    EXPECT_EQ(namespaced_topic("robot_2", "///a/b"), "/robot_2/a/b");
+}
+
+TEST(TopicNames, BodyTopicBaseUsesOverrideWhenSet)
+{
+    // Empty override → default perception/optitrack/{name}
+    EXPECT_EQ(body_topic_base("robot_1", "Drone", ""),
+              "/robot_1/perception/optitrack/Drone");
+    // Non-empty override → namespaced relative leaf (decoupled from body name)
+    EXPECT_EQ(body_topic_base("robot_1", "Drone", "perception/optitrack/drone"),
+              "/robot_1/perception/optitrack/drone");
+    EXPECT_EQ(body_topic_base("robot_3", "Target", "perception/optitrack/target"),
+              "/robot_3/perception/optitrack/target");
+}
+
+// ===========================================================================
+// Multi-body filtering — body_is_configured
+// ===========================================================================
+
+TEST(BodyIsConfigured, MatchesConfiguredIds)
+{
+    const std::vector<int32_t> ids = {1, 100};
+    EXPECT_TRUE(body_is_configured(ids, 1));
+    EXPECT_TRUE(body_is_configured(ids, 100));
+    EXPECT_FALSE(body_is_configured(ids, 2));
+}
+
+TEST(BodyIsConfigured, EmptySetMatchesNothing)
+{
+    const std::vector<int32_t> ids = {};
+    EXPECT_FALSE(body_is_configured(ids, 0));
+    EXPECT_FALSE(body_is_configured(ids, 1));
+}
+
 
 // ===========================================================================
 // Server negotiation — validate_connection_type
diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
index 913b58161..37526cd18 100644
--- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
+++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py
@@ -2,12 +2,14 @@
 # MIT License - see LICENSE in the repository root for full text.
 """Unit tests for natnet_ros2 Python helpers (no ROS install required).
 
-Stubs rclpy at import time. Covers ``VisionPoseConverterNode`` quaternion
-canonicalisation and frame_id assignment.
+Stubs rclpy/launch at import time. Covers ``VisionPoseConverterNode`` quaternion
+canonicalisation, configurable-topic wiring, and ``natnet_ros2.launch.py``
+profile-flattening helpers (server + per-body arrays, env expansion, namespacing).
 
 C++ logic (``natnet_logic.hpp``) is tested in ``test_natnet_logic.cpp`` via colcon.
 """
 
+import importlib.util
 import sys
 from pathlib import Path
 from types import SimpleNamespace
@@ -21,22 +23,31 @@
 # metaclass machinery returns a Mock for attribute access instead of running
 # __init_subclass__ / defining methods).  We supply a real dummy base class
 # so the actual class body — including _canonical_quaternion — is defined.
+#
+# The fake also records declared params and created sub/pub topics so the
+# configurable-topic wiring can be asserted without a ROS install.
 # ---------------------------------------------------------------------------
 
 class _FakeNode:
+    # Per-test parameter overrides keyed by name; consulted by declare_parameter so
+    # values survive the node's super().__init__ (which resets per-instance state).
+    _overrides: dict = {}
+
     def __init__(self, name: str):
-        pass
+        self._params: dict = {}
+        self.created_subscriptions: list = []
+        self.created_publishers: list = []
     def get_logger(self):
         return MagicMock()
-    def declare_parameter(self, *args, **kwargs):
-        pass
+    def declare_parameter(self, name, default=None):
+        self._params[name] = self._overrides.get(name, default)
     def get_parameter(self, name):
-        m = MagicMock()
-        m.value = MagicMock()
-        return m
-    def create_subscription(self, *args, **kwargs):
+        return SimpleNamespace(value=self._params.get(name))
+    def create_subscription(self, msg_type, topic, callback, qos):
+        self.created_subscriptions.append(topic)
         return MagicMock()
-    def create_publisher(self, *args, **kwargs):
+    def create_publisher(self, msg_type, topic, qos):
+        self.created_publishers.append(topic)
         return MagicMock()
 
 
@@ -55,6 +66,36 @@ def create_publisher(self, *args, **kwargs):
 from vision_pose_converter_node import VisionPoseConverterNode  # noqa: E402
 
 
+# ---------------------------------------------------------------------------
+# Load natnet_ros2.launch.py with its heavy launch/ROS deps stubbed, so the
+# pure flattening helpers can be unit-tested without a ROS install.
+# ---------------------------------------------------------------------------
+
+for _mod in (
+    "ament_index_python",
+    "ament_index_python.packages",
+    "launch",
+    "launch.actions",
+    "launch.launch_description_sources",
+    "launch.substitutions",
+    "launch_ros",
+    "launch_ros.actions",
+):
+    sys.modules.setdefault(_mod, MagicMock())
+
+# yaml is only needed by _load_natnet_config (not the flattening helpers); stub it
+# if PyYAML is absent so the launch module still imports in a minimal unit env.
+try:
+    import yaml  # noqa: F401
+except ImportError:
+    sys.modules.setdefault("yaml", MagicMock())
+
+_launch_path = Path(__file__).resolve().parent.parent / "launch" / "natnet_ros2.launch.py"
+_spec = importlib.util.spec_from_file_location("natnet_ros2_launch_under_test", _launch_path)
+natnet_launch = importlib.util.module_from_spec(_spec)
+_spec.loader.exec_module(natnet_launch)
+
+
 # ---------------------------------------------------------------------------
 # Helpers
 # ---------------------------------------------------------------------------
@@ -143,3 +184,104 @@ def test_canonical_quaternion_dual_sign_produces_same_result():
     assert out_pos.x == pytest.approx(out_neg.x)
     assert out_pos.y == pytest.approx(out_neg.y)
     assert out_pos.z == pytest.approx(out_neg.z)
+
+
+# ---------------------------------------------------------------------------
+# VisionPoseConverterNode — configurable input/output topics
+# ---------------------------------------------------------------------------
+
+@pytest.mark.unit
+def test_vision_pose_converter_default_topics():
+    """Defaults reproduce the historical relative (remappable) topic names."""
+    node = VisionPoseConverterNode()
+    assert node.created_subscriptions == ["input_pose"]
+    assert node.created_publishers == ["output_pose", "output_pose_cov"]
+
+
+@pytest.mark.unit
+def test_vision_pose_converter_topic_overrides_applied():
+    """When the topic params are set, sub/pub use those exact names."""
+    _FakeNode._overrides = {
+        "input_topic": "/robot_2/perception/optitrack/drone/pose_cov",
+        "output_pose_topic": "/robot_2/custom/vision/pose",
+        "output_pose_cov_topic": "/robot_2/custom/vision/pose_cov",
+    }
+    try:
+        node = VisionPoseConverterNode()
+    finally:
+        _FakeNode._overrides = {}
+    assert node.created_subscriptions == ["/robot_2/perception/optitrack/drone/pose_cov"]
+    assert node.created_publishers == [
+        "/robot_2/custom/vision/pose",
+        "/robot_2/custom/vision/pose_cov",
+    ]
+
+
+# ---------------------------------------------------------------------------
+# natnet_ros2.launch.py — pure config-flattening helpers
+# ---------------------------------------------------------------------------
+
+@pytest.mark.unit
+def test_expand_env_uses_default_when_unset(monkeypatch):
+    monkeypatch.delenv("NATNET_SERVER_IP", raising=False)
+    assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "172.31.0.200"
+
+
+@pytest.mark.unit
+def test_expand_env_uses_environment_value(monkeypatch):
+    monkeypatch.setenv("NATNET_SERVER_IP", "10.0.0.5")
+    assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "10.0.0.5"
+
+
+@pytest.mark.unit
+def test_namespaced_strips_and_prefixes():
+    assert natnet_launch._namespaced("robot_1", "perception/optitrack/drone") == \
+        "/robot_1/perception/optitrack/drone"
+    assert natnet_launch._namespaced("robot_2", "/already/abs") == "/robot_2/already/abs"
+
+
+@pytest.mark.unit
+def test_build_node_params_flattens_bodies():
+    server = {"server_ip": "1.2.3.4", "command_port": 1510, "connection_type": "unicast"}
+    profile = {
+        "bodies": [
+            {
+                "rigid_body_name": "Drone",
+                "id": 1,
+                "topic": "perception/optitrack/drone",
+                "pose": True,
+                "pose_cov": True,
+                "position_covariance": [9.0] * 9,
+                "orientation_covariance": [8.0] * 9,
+            },
+            {
+                "rigid_body_name": "Target",
+                "id": 100,
+                "topic": "perception/optitrack/target",
+                "pose": True,
+                "pose_cov": False,
+            },
+        ]
+    }
+    params = natnet_launch._build_node_params(server, profile)
+
+    assert params["server_ip"] == "1.2.3.4"
+    assert params["body_names"] == ["Drone", "Target"]
+    assert params["body_ids"] == [1, 100]
+    assert params["body_topics"] == ["perception/optitrack/drone", "perception/optitrack/target"]
+    assert params["body_pose"] == [True, True]
+    assert params["body_pose_cov"] == [True, False]
+    # 9 floats per body, flattened in body order.
+    assert len(params["body_position_covariance"]) == 18
+    assert params["body_position_covariance"][:9] == [9.0] * 9
+    # Target omitted its covariance → built-in default fills its slice.
+    assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE
+
+
+@pytest.mark.unit
+def test_build_node_params_empty_profile():
+    """A robot with no profile yields empty body arrays (node tracks nothing)."""
+    params = natnet_launch._build_node_params({}, {})
+    assert params["body_names"] == []
+    assert params["body_ids"] == []
+    assert params["body_position_covariance"] == []
diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml
index e433e94d4..354eb3c88 100644
--- a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml
+++ b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml
@@ -73,7 +73,8 @@
         </group>
 
         <group if="$(var launch_natnet)">
-            <!-- IPs / rigid-body IDs live in natnet_ros2/config/natnet_config.yaml (ROS 2 param file format). -->
+            <!-- Server IPs + per-robot body/vision_pose profiles live in
+                 natnet_ros2/config/natnet_config.yaml (custom natnet: schema, parsed by the launch file). -->
             <include file="$(find-pkg-share natnet_ros2)/launch/natnet_ros2.launch.py"/>
         </group>
 
diff --git a/simulation/isaac-sim/docker/sitl-files/px4-vision.env b/simulation/isaac-sim/docker/sitl-files/px4-vision.env
index 686748d98..39a6ebf67 100644
--- a/simulation/isaac-sim/docker/sitl-files/px4-vision.env
+++ b/simulation/isaac-sim/docker/sitl-files/px4-vision.env
@@ -4,7 +4,7 @@
 #   Isaac NatNet emulator → natnet_ros2 → vision_pose_converter
 #   → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2
 #
-# Pair with robot-side publish_to_mavros: true in natnet_config.yaml.
+# Pair with robot-side vision_pose.enabled: true in the robot's natnet_config.yaml profile.
 # Set SITL_PARAM_PROFILE=px4-vision in .env (or overrides/isaac-natnet-vision.env).
 
 # Fuse horizontal position, vertical position, and yaw from VISION_POSITION_ESTIMATE.
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
index 9ea3dc7cc..407c150b3 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md
@@ -71,7 +71,14 @@ Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridg
 
 **Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml).
 
-**Isaac Sim:** same `LAUNCH_NATNET` env var (forwarded in [`docker-compose.yaml`](../../docker/docker-compose.yaml)). When true, the Pegasus **multi** launch script authors the interface prim and auto-starts the server via `start_drone_natnet_server()` — one rigid body per drone `base_link`. Single-drone harness uses `example_multi_px4_pegasus_launch_script.py` even for `NUM_ROBOTS=1`.
+**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script):
+
+| Script | Use |
+|--------|-----|
+| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` |
+| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) |
+
+Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration.
 
 Convenience bundle for NatNet + external-vision PX4 SITL:
 
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
index 9a84e03f6..39b4eed7a 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md
@@ -378,18 +378,22 @@ Make the extension *aware* of interfaces without acting on them.
 >   `start_drone_natnet_server(stage, drones, ...)` (authors the `/World/NatNetInterface`
 >   prim, then creates a physics-subscribed `NatNetServerManager` and starts it).
 >   Exported from `optitrack.natnet.emulator.isaac`.
-> - **Launch scripts** (`launch_scripts/example_one*.py`, `example_multi*.py`): after
->   spawning drones, if `LAUNCH_NATNET=true` they author one rigid body per drone
->   `base_link` and start the server. Single: `("Drone", 1, /World/base_link)`.
->   Multi: `("<NATNET_BODY_NAME><i>", i, /World/drone<i>/base_link)` for each drone.
->   Failures are caught (never kill the sim); the manager is torn down on close.
+> - **Launch scripts** (NatNet line: `launch_scripts/example_one_px4_pegasus_natnet_launch_script.py`,
+>   `example_multi_px4_pegasus_natnet_launch_script.py`): after spawning drones, author one
+>   rigid body per drone `base_link`, a static `/World/target` body (via `author_static_target`),
+>   and start the server. Single: `("Drone", 1, /World/base_link/body)` +
+>   `("Target", 100, /World/target)`. Multi: `("<NATNET_BODY_NAME><i>", i, /World/drone<i>/base_link/body)`
+>   per drone + one shared `("Target", 100, /World/target)`. Each robot's `natnet_config.yaml`
+>   profile selects which bodies it subscribes to. Failures are caught (never kill the sim);
+>   the manager is torn down on close.
+>   Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`,
+>   `example_multi_px4_pegasus_launch_script.py`) have no NatNet code.
 > - **Import path**: scripts add `../extensions/optitrack.natnet.emulator` to
 >   `sys.path` so the package imports without enabling the Kit UI extension (keeps a
 >   single, script-owned manager — no duplicate physics/USD subscriptions).
-> - **Wiring**: `LAUNCH_NATNET` / `NATNET_BODY_NAME` are passed into the isaac-sim
->   container via `simulation/isaac-sim/docker/docker-compose.yaml`, mirroring the
->   robot-side `natnet_ros2` gate. Server binds the container IP `172.31.0.200`
->   (the config default), which the robot `server_ip` already points at.
+> - **Wiring:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script; `NATNET_BODY_NAME` /
+>   `NATNET_TARGET_NAME` optional. Robot-side `LAUNCH_NATNET=true` brings up `natnet_ros2`.
+>   Server binds the container IP `172.31.0.200` (the config default).
 > - **Tests**: `test_scene_setup.py` (pure: single/multi mapping, server-param
 >   forwarding, duplicate name/id rejection, empty catalog). Proxy under
 >   `tests/sim/optitrack_natnet_emulator/`.
@@ -587,23 +591,24 @@ Hard-won findings from getting the liveliness sentinel green (Isaac emulator →
    default `auto_stream=True` path still works. Don't "optimize" the pump back into
    the daemon thread.
 
-2. **The liveliness/system path runs the *standalone Pegasus script*, not a saved
-   scene USD.** `tests/conftest.py` sets `ISAAC_SIM_USE_STANDALONE=true` **and
-   `ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py`** — even for
-   `--num-robots 1`. So the NatNet auto-start wiring **must** live in
-   `example_multi_…` (it does); `example_one_…` and the default
-   `airstack up isaac-sim` (which uses `ISAAC_SIM_USE_STANDALONE=false` →
-   `run_isaacsim.launch.py` opening `simple_pegasus.scene.usd`) do **not** exercise
-   it. To reproduce the harness manually you must pass all of
+2. **NatNet liveliness uses a NatNet Pegasus script, not the baseline scripts.**
+   Default system tests (`tests/conftest.py`) use
+   `ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py` (no NatNet).
+   For the NatNet pose sentinel, set robot `LAUNCH_NATNET=true` **and** point sim at
+   `example_multi_px4_pegasus_natnet_launch_script.py` (or the one-drone NatNet
+   script). Convenience bundle: `overrides/isaac-natnet-vision.env`.
+
+   Manual repro:
    `AUTOLAUNCH=true ISAAC_SIM_USE_STANDALONE=true
-   ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py
+   ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_natnet_launch_script.py
    LAUNCH_NATNET=true PLAY_SIM_ON_START=true`.
 
 3. **Single-agent body/prim contract.** For `NUM_ROBOTS=1` the multi script names
    the body `"Drone"` (bare) → target `/World/drone1/base_link` (the multi script's
-   drone prim path is always `drone{i}`, even for one drone). This matches the robot
-   `natnet_config.yaml` body and the liveliness sentinel topic
-   `/robot_1/perception/optitrack/Drone/pose_cov`.
+   drone prim path is always `drone{i}`, even for one drone). The robot's
+   `natnet_config.yaml` profile maps the `Drone` body (id 1) to the relative topic
+   `perception/optitrack/drone`, so the liveliness sentinel topic is
+   `/robot_1/perception/optitrack/drone/pose_cov`.
 
 4. **Target prims appear *after* server start.** The Pegasus `base_link` is created
    on the first Play tick — after `start_server()`. `_sample_cache` therefore stores
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
index 747831997..2c1915264 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py
@@ -15,7 +15,7 @@ class TrackedBodyBinding:
     parent_id: int = -1
 
 
-# Single-drone Pegasus scenes (example_one_px4_pegasus_launch_script.py).
+# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py).
 DEFAULT_DRONE_BINDING = TrackedBodyBinding(
     name="Drone",
     id=1,
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
index 2812257f1..ffd1b9e93 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py
@@ -17,6 +17,10 @@
 from .manager import NatNetServerManager, default_server_factory, format_interface
 from .scene_setup import (
     DEFAULT_INTERFACE_PATH,
+    DEFAULT_TARGET_PATH,
+    DEFAULT_TARGET_POSITION,
+    DEFAULT_TARGET_STREAMING_ID,
+    author_static_target,
     build_drone_config,
     start_drone_natnet_server,
 )
@@ -31,11 +35,15 @@
 
 __all__ = [
     "DEFAULT_INTERFACE_PATH",
+    "DEFAULT_TARGET_PATH",
+    "DEFAULT_TARGET_POSITION",
+    "DEFAULT_TARGET_STREAMING_ID",
     "BodyBinding",
     "BodySample",
     "NatNetInterfaceConfig",
     "NatNetServerManager",
     "author_interface",
+    "author_static_target",
     "body_attr_name",
     "build_catalog",
     "build_drone_config",
diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
index bf22065dd..d7e804c61 100644
--- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
+++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py
@@ -33,10 +33,36 @@
 # Where the example scripts author the single interface prim.
 DEFAULT_INTERFACE_PATH = "/World/NatNetInterface"
 
+# Default world prim + position for the demo "target" body (a static placeholder
+# the example scripts stream alongside the drones so a tracked target is available).
+DEFAULT_TARGET_PATH = "/World/target"
+DEFAULT_TARGET_POSITION = (2.0, 0.0, 1.0)
+DEFAULT_TARGET_STREAMING_ID = 100
+
 # (rigid_body_name, streaming_id, target_prim_path)
 DroneSpec = Tuple[str, int, str]
 
 
+def author_static_target(
+    stage,
+    prim_path: str = DEFAULT_TARGET_PATH,
+    position: Sequence[float] = DEFAULT_TARGET_POSITION,
+):
+    """Author a static ``Xform`` prim to act as a NatNet-tracked target.
+
+    Creates ``prim_path`` (a plain transform with a single translate op) at
+    ``position`` so the emulator can sample it like any other tracked body. The
+    prim is static — no physics, no animation — representing a fixed point of
+    interest that drones can be commanded toward. Imports ``pxr`` lazily so this
+    module stays importable outside Isaac. Returns ``prim_path``.
+    """
+    from pxr import Gf, UsdGeom
+
+    xform = UsdGeom.Xform.Define(stage, prim_path)
+    xform.AddTranslateOp().Set(Gf.Vec3d(float(position[0]), float(position[1]), float(position[2])))
+    return prim_path
+
+
 def build_drone_config(
     drones: Iterable[DroneSpec],
     *,
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
index 2fdd21e80..0c9c7b082 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py
@@ -39,7 +39,6 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
-from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
 # --------------------- CONFIGURATION ---------------------
 ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
@@ -49,14 +48,6 @@
 
 NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1"))
 ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true"
-
-# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface
-# prim with every drone's base_link as a tracked rigid body and start the server on
-# load. Mirrors the robot-side LAUNCH_NATNET gate. Each drone i is streamed as
-# "<NATNET_BODY_NAME><i>" (streaming id i); with the default natnet_config.yaml
-# (body_id=-1) each robot publishes all bodies, so narrow per robot with body_id.
-LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on")
-NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
 # ---------------------------------------------------------
 
 
@@ -181,43 +172,8 @@ def __init__(self):
         for i in range(1, NUM_ROBOTS + 1):
             spawn_drone(i)
 
-        # ----- OptiTrack / NatNet emulator -----
-        # Author a NatNet interface prim with every drone's base_link as a tracked
-        # rigid body and start the Motive-compatible server.
-        self.natnet_manager = None
-        if LAUNCH_NATNET:
-            self._setup_natnet(stage)
-
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
 
-    def _setup_natnet(self, stage):
-        """Author the NatNet interface prim (one body per drone) and start the server."""
-        try:
-            # Single agent: stream the bare body name ("Drone") so it matches the
-            # robot natnet_config body_name, the MAVROS vision-pose bridge, and the
-            # liveliness sentinel. Multi-drone uses unique indexed names (future:
-            # narrow each robot to its own body via body_id).
-            def _body_name(i: int) -> str:
-                return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{i}"
-
-            drones = [
-                (_body_name(i), i, f"/World/drone{i}/base_link/body")
-                for i in range(1, NUM_ROBOTS + 1)
-            ]
-            self.natnet_manager = start_drone_natnet_server(
-                stage,
-                drones,
-                **{
-                    "pose_noise_enabled": True,
-                    "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm
-                    "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg
-                }
-            )
-            carb.log_warn(f"[natnet] Emulator started with {len(drones)} body(ies).")
-        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
-            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
-            self.natnet_manager = None
-
     def run(self):
         if self.play_on_start:
             self.timeline.play()
@@ -236,8 +192,6 @@ def run(self):
                 app.update()
 
         carb.log_warn("Closing simulation.")
-        if self.natnet_manager is not None:
-            self.natnet_manager.on_shutdown()
         self.timeline.stop()
         simulation_app.close()
 
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py
new file mode 100644
index 000000000..12b4799d1
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py
@@ -0,0 +1,256 @@
+#!/usr/bin/env python
+"""
+Multi-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming.
+
+Same scene prep and sensor stack as ``example_multi_px4_pegasus_launch_script.py``,
+plus a Motive-compatible NatNet server that always streams one rigid body per drone
+and a shared static ``Target`` (id 100) at ``/World/target``.
+
+Body naming:
+ - ``NUM_ROBOTS=1``: drone body ``Drone`` (id 1)
+ - ``NUM_ROBOTS>1``: ``Drone1``, ``Drone2``, … (ids 1..N)
+
+Intended multi-robot profile pairing (see ``natnet_config.yaml`` commented scaffolding):
+ - ``robot_1``: tracks its drone + the shared ``Target``
+ - ``robot_2``: tracks its drone + the shared ``Target``
+ - ``robot_3``: tracks its drone only (no Target in profile)
+
+Set ``NUM_ROBOTS=3`` on both sim and robot stacks; each container picks its profile
+via ``ROBOT_NAME``.
+
+Env:
+ - ``NUM_ROBOTS`` (default 1)
+ - ``ENABLE_LIDAR`` (default false)
+ - ``PLAY_SIM_ON_START`` (default true)
+ - ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME`` (optional name overrides)
+"""
+
+import asyncio
+import os
+import sys
+import time
+
+import carb
+from isaacsim import SimulationApp
+
+_headless = os.environ.get("ISAAC_SIM_HEADLESS", "false").lower() == "true"
+simulation_app = SimulationApp({"headless": _headless})
+
+import omni.kit.app
+import omni.timeline
+import omni.usd
+
+from omni.isaac.core.world import World
+
+from pegasus.simulator.params import SIMULATION_ENVIRONMENTS
+from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
+from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node
+from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph
+from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph
+
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
+
+from optitrack.natnet.emulator.isaac import (
+    DEFAULT_TARGET_PATH,
+    DEFAULT_TARGET_POSITION,
+    DEFAULT_TARGET_STREAMING_ID,
+    author_static_target,
+    start_drone_natnet_server,
+)
+
+# --------------------- CONFIGURATION ---------------------
+ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
+STAGE_SCALE = 1.0
+SAVE_SCENE_TO = None
+DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
+
+NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1"))
+ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true"
+NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
+NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target")
+
+_NATNET_SERVER_KWARGS = {
+    "pose_noise_enabled": True,
+    "pose_noise_std_meters": 0.0005,
+    "pose_noise_rotation_deg": 0.05,
+}
+# ---------------------------------------------------------
+
+
+ext_manager = omni.kit.app.get_app().get_extension_manager()
+for ext in [
+    "omni.graph.core",
+    "omni.graph.action",
+    "omni.graph.action_nodes",
+    "isaacsim.core.nodes",
+    "omni.graph.ui",
+    "omni.graph.visualization.nodes",
+    "omni.graph.scriptnode",
+    "omni.graph.window.action",
+    "omni.graph.window.generic",
+    "omni.graph.ui_nodes",
+    "pegasus.simulator",
+]:
+    if not ext_manager.is_extension_enabled(ext):
+        ext_manager.set_extension_enabled_immediate(ext, True)
+
+
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
+
+
+def _drone_body_name(index: int) -> str:
+    """Single agent uses bare ``Drone``; multi uses ``Drone1``, ``Drone2``, …"""
+    return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{index}"
+
+
+def spawn_drone(index: int):
+    robot_name = f"robot_{index}"
+    drone_prim = f"/World/drone{index}/base_link"
+    init_x = 2.0 * (index - 1) - 2.0 * (NUM_ROBOTS - 1) / 2.0
+
+    graph_handle = spawn_px4_multirotor_node(
+        pegasus_node_name=f"PX4Multirotor_{index}",
+        drone_prim=drone_prim,
+        robot_name=robot_name,
+        vehicle_id=index,
+        domain_id=index,
+        usd_file=DRONE_USD,
+        init_pos=[init_x, 0.0, 0.07],
+        init_orient=[0.0, 0.0, 0.0, 1.0],
+    )
+
+    add_zed_stereo_camera_subgraph(
+        parent_graph_handle=graph_handle,
+        drone_prim=drone_prim,
+        robot_name=robot_name,
+        camera_name="ZEDCamera",
+        camera_offset=[0.2, 0.0, -0.05],
+        camera_rotation_offset=[0.0, 0.0, 0.0],
+    )
+
+    if ENABLE_LIDAR:
+        add_rtx_lidar_subgraph(
+            parent_graph_handle=graph_handle,
+            drone_prim=drone_prim,
+            robot_name=robot_name,
+            lidar_config="ouster_os1",
+            lidar_topic_name="point_cloud_raw",
+            lidar_offset=[0.0, 0.0, 0.025],
+            lidar_rotation_offset=[0.0, 0.0, 0.0],
+            min_range=0.75,
+        )
+
+
+class PegasusApp:
+
+    def __init__(self):
+        self.timeline = omni.timeline.get_timeline_interface()
+        self.natnet_manager = None
+
+        self.pg = PegasusInterface()
+        self.pg._world = World(**self.pg._world_settings)
+        self.world = self.pg.world
+        self.timeline.stop()
+
+        self.pg.load_environment(ENV_URL)
+
+        stage = omni.usd.get_context().get_stage()
+        if stage is None:
+            raise RuntimeError("Stage failed to load")
+
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
+
+        stage_prim = stage.GetPrimAtPath("/World/stage")
+        if stage_prim.IsValid():
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
+            for _ in range(10):
+                omni.kit.app.get_app().update()
+        else:
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
+
+        add_dome_light(stage)
+
+        if SAVE_SCENE_TO:
+            import tempfile
+            tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd")
+            success, error = asyncio.get_event_loop().run_until_complete(
+                omni.usd.get_context().export_as_stage_async(tmp_usd)
+            )
+            if success:
+                os.makedirs(SAVE_SCENE_TO, exist_ok=True)
+                save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO)
+                os.remove(tmp_usd)
+            else:
+                carb.log_error(f"Scene export failed: {error}")
+
+        print(f"[example_multi_natnet] Spawning {NUM_ROBOTS} drone(s), lidar={'on' if ENABLE_LIDAR else 'off'}")
+        for i in range(1, NUM_ROBOTS + 1):
+            spawn_drone(i)
+
+        self._setup_natnet(stage)
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
+
+    def _setup_natnet(self, stage):
+        """Author NatNet bodies: one per drone plus one shared static target."""
+        try:
+            author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION)
+            bodies = [
+                (_drone_body_name(i), i, f"/World/drone{i}/base_link/body")
+                for i in range(1, NUM_ROBOTS + 1)
+            ]
+            bodies.append((NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH))
+
+            self.natnet_manager = start_drone_natnet_server(
+                stage, bodies, **_NATNET_SERVER_KWARGS
+            )
+            carb.log_warn(
+                f"[natnet] Emulator started with {NUM_ROBOTS} drone body(ies) "
+                f"and shared target '{NATNET_TARGET_NAME}' (robot_1/robot_2 subscribe via "
+                f"natnet_config; robot_3 omits Target)."
+            )
+        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
+            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
+            self.natnet_manager = None
+
+    def run(self):
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
+
+        app = omni.kit.app.get_app()
+        while simulation_app.is_running():
+            world = World.instance()
+            if world is not None and hasattr(world, '_scene'):
+                world.step(render=True)
+                if world is not self.world:
+                    self.world = world
+                    self.pg._world = world
+            else:
+                app.update()
+
+        carb.log_warn("Closing simulation.")
+        if self.natnet_manager is not None:
+            self.natnet_manager.on_shutdown()
+        self.timeline.stop()
+        simulation_app.close()
+
+
+def main():
+    PegasusApp().run()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
index f4dad4f89..11819fc2f 100755
--- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py
@@ -97,19 +97,11 @@
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
 
-from optitrack.natnet.emulator.isaac import start_drone_natnet_server
 
 # --------------------- CONFIGURATION ---------------------
 # Environment to load. Swap this URL/key for any other scene.
 ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
 
-# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface
-# prim with the drone's base_link as a tracked rigid body and start the server on
-# load. Mirrors the robot-side LAUNCH_NATNET gate (natnet_ros2 client). The body
-# name defaults to "Drone" to match natnet_config.yaml / the liveliness sentinel.
-LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on")
-NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
-
 # Scale applied to /World/stage. 0.01 converts cm→m for Nucleus assets.
 # Set to 1.0 if the environment is already in meters.
 STAGE_SCALE = 1.0
@@ -252,35 +244,8 @@ def __init__(self):
             min_range=0.75,
         )
 
-        # ----- OptiTrack / NatNet emulator -----
-        # Author a NatNet interface prim with the drone's base_link as a tracked
-        # rigid body and start the Motive-compatible server.
-        self.natnet_manager = None
-        if LAUNCH_NATNET:
-            self._setup_natnet(stage)
-
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
 
-    def _setup_natnet(self, stage):
-        """Author the NatNet interface prim and start the emulator server."""
-        try:
-            drones = [(NATNET_BODY_NAME, 1, "/World/base_link/body")]
-            self.natnet_manager = start_drone_natnet_server(
-                stage, 
-                drones,
-                **{
-                    "pose_noise_enabled": True,
-                    "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm
-                    "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg
-                }
-            )
-            carb.log_warn(
-                f"[natnet] Emulator started with 1 body ('{NATNET_BODY_NAME}' -> /World/base_link)."
-            )
-        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
-            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
-            self.natnet_manager = None
-
     def run(self):
 
         if self.play_on_start:
@@ -302,8 +267,6 @@ def run(self):
                 app.update()
 
         carb.log_warn("Closing simulation.")
-        if self.natnet_manager is not None:
-            self.natnet_manager.on_shutdown()
         self.timeline.stop()
         simulation_app.close()
 
diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py
new file mode 100644
index 000000000..f2957108b
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py
@@ -0,0 +1,250 @@
+#!/usr/bin/env python
+"""
+Single-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming.
+
+Same scene prep and sensor stack as ``example_one_px4_pegasus_launch_script.py``, plus
+a Motive-compatible NatNet server that always streams:
+
+ - ``Drone`` (id 1) from the Pegasus ``body`` prim under ``/World/base_link``
+ - ``Target`` (id 100) from a static ``/World/target`` prim
+
+Pair with robot-side ``LAUNCH_NATNET=true`` and a matching ``natnet_config.yaml``
+profile. To consume the target on the robot, add a Target body to the profile
+(see the commented scaffolding in ``natnet_config.yaml``).
+
+Override rigid-body names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``.
+"""
+
+import os
+import sys
+import time
+import asyncio
+
+import carb
+from isaacsim import SimulationApp
+
+_LIVESTREAM = os.environ.get("ISAAC_SIM_LIVESTREAM", "").lower() == "true"
+
+if _LIVESTREAM:
+    _SIM_APP_CONFIG = {
+        "width": 1280,
+        "height": 720,
+        "window_width": 1920,
+        "window_height": 1080,
+        "headless": True,
+        "hide_ui": False,
+        "renderer": "RaytracedLighting",
+        "display_options": 3286,
+    }
+else:
+    _SIM_APP_CONFIG = {"headless": False}
+
+simulation_app = SimulationApp(launch_config=_SIM_APP_CONFIG)
+
+if _LIVESTREAM:
+    from isaacsim.core.utils.extensions import enable_extension
+    simulation_app.set_setting("/app/window/drawMouse", True)
+    simulation_app.set_setting("/app/livestream/enabled", True)
+    LIVESTREAM_UDP_PORT = int(os.environ.get("ISAAC_SIM_LIVESTREAM_UDP_PORT", "49099"))
+    simulation_app.set_setting("/app/livestream/fixedHostPort", LIVESTREAM_UDP_PORT)
+    simulation_app.set_setting("/app/livestream/minHostPort", LIVESTREAM_UDP_PORT)
+    simulation_app.set_setting("/app/livestream/maxHostPort", LIVESTREAM_UDP_PORT)
+    enable_extension("omni.kit.livestream.webrtc")
+
+import omni.kit.app
+import omni.timeline
+import omni.usd
+
+from omni.isaac.core.world import World
+
+from pegasus.simulator.params import SIMULATION_ENVIRONMENTS
+from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
+from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node
+from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph
+from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph
+
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd
+
+from optitrack.natnet.emulator.isaac import (
+    DEFAULT_TARGET_PATH,
+    DEFAULT_TARGET_POSITION,
+    DEFAULT_TARGET_STREAMING_ID,
+    author_static_target,
+    start_drone_natnet_server,
+)
+
+# --------------------- CONFIGURATION ---------------------
+ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"]
+STAGE_SCALE = 1.0
+SAVE_SCENE_TO = None
+DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
+
+NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
+NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target")
+
+_NATNET_SERVER_KWARGS = {
+    "pose_noise_enabled": True,
+    "pose_noise_std_meters": 0.0005,
+    "pose_noise_rotation_deg": 0.05,
+}
+# ---------------------------------------------------------
+
+
+ext_manager = omni.kit.app.get_app().get_extension_manager()
+for ext in [
+    "omni.graph.core",
+    "omni.graph.action",
+    "omni.graph.action_nodes",
+    "isaacsim.core.nodes",
+    "omni.graph.ui",
+    "omni.graph.visualization.nodes",
+    "omni.graph.scriptnode",
+    "omni.graph.window.action",
+    "omni.graph.window.generic",
+    "omni.graph.ui_nodes",
+    "pegasus.simulator",
+]:
+    if not ext_manager.is_extension_enabled(ext):
+        ext_manager.set_extension_enabled_immediate(ext, True)
+
+
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
+
+
+class PegasusApp:
+
+    def __init__(self):
+        self.timeline = omni.timeline.get_timeline_interface()
+        self.natnet_manager = None
+
+        self.pg = PegasusInterface()
+        self.pg._world = World(**self.pg._world_settings)
+        self.world = self.pg.world
+        self.timeline.stop()
+
+        self.pg.load_environment(ENV_URL)
+
+        stage = omni.usd.get_context().get_stage()
+        if stage is None:
+            raise RuntimeError("Stage failed to load")
+
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
+
+        stage_prim = stage.GetPrimAtPath("/World/stage")
+        if stage_prim.IsValid():
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
+            for _ in range(10):
+                omni.kit.app.get_app().update()
+        else:
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
+
+        add_dome_light(stage)
+
+        if SAVE_SCENE_TO:
+            import tempfile
+            tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd")
+            success, error = asyncio.get_event_loop().run_until_complete(
+                omni.usd.get_context().export_as_stage_async(tmp_usd)
+            )
+            if success:
+                os.makedirs(SAVE_SCENE_TO, exist_ok=True)
+                save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO)
+                os.remove(tmp_usd)
+            else:
+                carb.log_error(f"Scene export failed: {error}")
+
+        graph_handle = spawn_px4_multirotor_node(
+            pegasus_node_name="PX4Multirotor",
+            drone_prim="/World/base_link",
+            robot_name="robot_1",
+            vehicle_id=1,
+            domain_id=1,
+            usd_file=DRONE_USD,
+            init_pos=[0.0, 0.0, 0.07],
+            init_orient=[0.0, 0.0, 0.0, 1.0],
+        )
+
+        add_zed_stereo_camera_subgraph(
+            parent_graph_handle=graph_handle,
+            drone_prim="/World/base_link",
+            robot_name="robot_1",
+            camera_name="ZEDCamera",
+            camera_offset=[0.2, 0.0, -0.05],
+            camera_rotation_offset=[0.0, 0.0, 0.0],
+        )
+
+        add_rtx_lidar_subgraph(
+            parent_graph_handle=graph_handle,
+            drone_prim="/World/base_link",
+            robot_name="robot_1",
+            lidar_config="ouster_os1",
+            lidar_topic_name="point_cloud_raw",
+            lidar_offset=[0.0, 0.0, 0.025],
+            lidar_rotation_offset=[0.0, 0.0, 0.0],
+            min_range=0.75,
+        )
+
+        self._setup_natnet(stage)
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
+
+    def _setup_natnet(self, stage):
+        """Author the NatNet interface prim (drone + static target) and start the server."""
+        try:
+            author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION)
+            bodies = [
+                (NATNET_BODY_NAME, 1, "/World/base_link/body"),
+                (NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH),
+            ]
+            self.natnet_manager = start_drone_natnet_server(
+                stage, bodies, **_NATNET_SERVER_KWARGS
+            )
+            carb.log_warn(
+                f"[natnet] Emulator started: '{NATNET_BODY_NAME}' (-> /World/base_link/body), "
+                f"'{NATNET_TARGET_NAME}' (-> {DEFAULT_TARGET_PATH})."
+            )
+        except Exception as exc:  # noqa: BLE001 - never let NatNet kill the sim
+            carb.log_error(f"[natnet] Failed to start emulator: {exc}")
+            self.natnet_manager = None
+
+    def run(self):
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
+
+        app = omni.kit.app.get_app()
+        while simulation_app.is_running():
+            world = World.instance()
+            if world is not None and hasattr(world, '_scene'):
+                world.step(render=True)
+                if world is not self.world:
+                    self.world = world
+                    self.pg._world = world
+            else:
+                app.update()
+
+        carb.log_warn("Closing simulation.")
+        if self.natnet_manager is not None:
+            self.natnet_manager.on_shutdown()
+        self.timeline.stop()
+        simulation_app.close()
+
+
+def main():
+    PegasusApp().run()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md
index 74502273f..d0085a63f 100644
--- a/tests/integration/natnet/README.md
+++ b/tests/integration/natnet/README.md
@@ -12,22 +12,23 @@ For the **in-sim** end-to-end check (Isaac emulator + full stack), see
 
 ## What it verifies
 
-Two variants in [`test_natnet_integration.py`](test_natnet_integration.py).
-Both start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the
+Three variants in [`test_natnet_integration.py`](test_natnet_integration.py).
+All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the
 robot container pointed at the Docker bridge gateway, and assert a sustained
-pose stream at **≥ 5 Hz** on:
+pose stream at **≥ 5 Hz** on the configured topic(s), e.g.:
 
-- `/{ROBOT_NAME}/perception/optitrack/Drone/pose_cov` (wait for first message)
-- `/{ROBOT_NAME}/perception/optitrack/Drone` (Hz sample)
+- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message)
+- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample)
 
 | Test | Path |
 |------|------|
 | **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. |
 | **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. |
+| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. |
 
 These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they
-exec `natnet_ros2_node` directly with `body_name:=Drone`, `body_id:=1`, and
-`publish_to_mavros:=false`.
+exec `natnet_ros2_node` directly with the flattened per-body params
+(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge.
 
 ## Requirements
 
@@ -74,13 +75,14 @@ On CI / PR (write access): `/pytest -m integration --run-integration`
 ┌────────────────────────────▼─────────────────────────────────┐
 │ Robot container (robot-desktop)                              │
 │  natnet_ros2_node (libNatNet 4.4 client)                     │
-│  → /{ROBOT_NAME}/perception/optitrack/Drone[/pose_cov]       │
+│  → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │
 └──────────────────────────────────────────────────────────────┘
 ```
 
 **In sim (liveliness tier):** the server runs inside the Isaac Sim container
-(`172.31.0.200` by default). Pegasus launch scripts call
-`start_drone_natnet_server()` when `LAUNCH_NATNET=true`; `natnet_ros2` in the
+(`172.31.0.200` by default). Use a NatNet Pegasus launch script
+(`example_one_px4_pegasus_natnet_launch_script.py` or
+`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the
 robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP).
 
 **Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only
@@ -98,16 +100,16 @@ The matching **system** check is
 
 - **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs
   are unaffected).
-- Asserts `/{robot_n}/perception/optitrack/{body}/pose_cov` ≥ 5 Hz per robot.
-- Override body name with `NATNET_BODY_NAME` (default `Drone`; multi-drone sim
-  uses indexed names like `Drone1` — align with
-  `example_multi_px4_pegasus_launch_script.py` and per-robot `natnet_config`).
-
-Sim auto-start is wired: with `LAUNCH_NATNET=true`, the Pegasus **multi** launch
-script (used by the system-test harness even for `NUM_ROBOTS=1`) authors
-`/World/NatNetInterface` and starts `NatNetServerManager` via
-`start_drone_natnet_server()`. Convenience env bundle:
-`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet + PX4 external-vision SITL profile).
+- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone
+  body's configured topic — default `perception/optitrack/drone`).
+- Override the checked topic with `NATNET_POSE_TOPIC` (default
+  `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default
+  `Drone`) is decoupled from the published topic, which the robot profile sets.
+
+Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and
+`LAUNCH_NATNET=true` on the robot. Convenience bundle:
+`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script +
+PX4 external-vision SITL profile).
 
 ## libNatNet 4.4 unicast — verified wire contract
 
diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py
index 427be2d78..2ac769501 100644
--- a/tests/integration/natnet/test_natnet_integration.py
+++ b/tests/integration/natnet/test_natnet_integration.py
@@ -2,12 +2,18 @@
 # MIT License - see LICENSE in the repository root for full text.
 """NatNet - robot autonomy integration tests.
 
-Two host-side variants stream Drone frames to ``natnet_ros2_node`` in the robot
-container and assert pose topics stay alive at >= 5 Hz: (1) raw
-``NatNetUnicastServer`` hand-built frames; (2) ``NatNetServerManager`` sampling
-an in-memory USD stage (Isaac wrapper path, no sim/GPU).
-
-In-sim end-to-end: ``tests/system/test_liveliness.py::test_natnet_pose_alive``.
+Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and
+assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built
+single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage
+(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that
+exercises per-body topic overrides and the pose / pose_cov toggles.
+
+The node is parameterised with the flattened per-body arrays
+(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``)
+that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile.
+
+Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by
+``tests/system/test_liveliness.py::test_natnet_pose_alive``.
 """
 
 from __future__ import annotations
@@ -80,38 +86,59 @@ def _stop_stale_natnet_nodes(container: str) -> None:
     time.sleep(0.5)
 
 
-def _make_drone_frame(frame_num: int) -> dt.sFrameOfMocapData:
+# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only;
+# the node maps ids → topics via its body_* params.
+_DRONE_BODY = (1, "Drone")
+_TARGET_BODY = (100, "Target")
+
+
+def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData:
     frame = dt.sFrameOfMocapData()
     frame.iFrame = frame_num
-    frame.nRigidBodies = 1
-    rb = frame.RigidBodies[0]
-    rb.ID = 1
-    rb.qw = 1.0
-    # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp).
-    rb.params = 1
+    frame.nRigidBodies = len(body_ids)
+    for slot, body_id in enumerate(body_ids):
+        rb = frame.RigidBodies[slot]
+        rb.ID = body_id
+        rb.qw = 1.0
+        # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp).
+        rb.params = 1
     return frame
 
 
-def _frame_publisher(server: NatNetUnicastServer, stop_event: threading.Event) -> None:
+def _frame_publisher(
+    server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,)
+) -> None:
     frame_num = 0
     interval = 1.0 / server.publish_rate
     while not stop_event.is_set():
-        server.enqueue_mocap_data(_make_drone_frame(frame_num))
+        server.enqueue_mocap_data(_make_frame(frame_num, body_ids))
         frame_num += 1
         time.sleep(interval)
 
 
-def _launch_natnet_node(container, host_ip, command_port, domain_id):
-    """Start natnet_ros2_node in the container pointed at the host emulator."""
+def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None):
+    """Start natnet_ros2_node in the container pointed at the host emulator.
+
+    ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single
+    Drone body on topic ``perception/optitrack/drone`` (the shipped config default).
+    """
+    if bodies is None:
+        bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")]
+    ids = ",".join(str(b[0]) for b in bodies)
+    names = ",".join(b[1] for b in bodies)
+    topics = ",".join(b[2] for b in bodies)
+    pose = ",".join(b[3] for b in bodies)
+    pose_cov = ",".join(b[4] for b in bodies)
     launch_cmd = (
         f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && "
         f"exec {_NATNET_NODE} --ros-args "
         f"-p server_ip:={host_ip} "
         f"-p command_port:={command_port} "
-        f"-p body_name:=Drone "
-        f"-p body_id:=1 "
-        f"-p publish_to_mavros:=false "
-        f"-p publish_direct_optitrack:=true'"
+        f"-p body_names:=[{names}] "
+        f"-p body_ids:=[{ids}] "
+        f"-p body_topics:=[{topics}] "
+        f"-p body_pose:=[{pose}] "
+        f"-p body_pose_cov:=[{pose_cov}]'"
     )
     return subprocess.Popen(
         ["docker", "exec", container, "bash", "-c", launch_cmd],
@@ -121,9 +148,9 @@ def _launch_natnet_node(container, host_ip, command_port, domain_id):
     )
 
 
-def _assert_pose_stream(container, robot_name, domain_id):
+def _assert_pose_stream(container, robot_name, domain_id, topic="perception/optitrack/drone"):
     """Wait for the pose topic then assert a sustained rate >= _MIN_HZ."""
-    pose_topic = f"/{robot_name}/perception/optitrack/Drone"
+    pose_topic = f"/{robot_name}/{topic}"
     pose_cov_topic = f"{pose_topic}/pose_cov"
 
     time.sleep(_WARMUP_S)
@@ -273,3 +300,69 @@ def _sampler():
         sampler.join(timeout=2.0)
         _terminate(node_proc)
         manager.stop_server()
+
+
+def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack):
+    """Multi-body profile: one robot tracks a drone + a static target.
+
+    Streams two bodies (drone id 1, target id 100) and configures the node like a
+    robot profile with two bodies and distinct relative topics. Asserts: the drone
+    pose streams >= 5 Hz on its custom topic; the target pose streams on its own
+    topic; and the target's pose_cov topic is absent (body_pose_cov=false).
+    """
+    container = robot_autonomy_stack["container"]
+
+    if not _natnet_node_available(container):
+        pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)")
+
+    _stop_stale_natnet_nodes(container)
+
+    host_ip = _docker_default_gateway(container)
+    command_port = ephemeral_udp_port(host_ip)
+    robot_name = _container_env(container, "ROBOT_NAME", "robot_1")
+    domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0"))
+
+    server = NatNetUnicastServer(
+        local_interface=host_ip,
+        transmission_type=TransmissionType.UNICAST,
+        multicast_address=None,
+        command_port=command_port,
+    )
+    server.publish_rate = 50
+
+    bodies = [
+        (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"),
+        (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"),
+    ]
+    body_ids = (_DRONE_BODY[0], _TARGET_BODY[0])
+
+    stop_event = threading.Event()
+    publisher = threading.Thread(
+        target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True
+    )
+
+    node_proc: subprocess.Popen[str] | None = None
+    try:
+        publisher.start()
+        time.sleep(0.1)
+        server.start()
+        node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies)
+        # Drone (pose + pose_cov) and target (pose only) both stream.
+        _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone")
+        _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/target")
+
+        # body_pose_cov=false → the target pose_cov publisher must not exist.
+        target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov"
+        topics = docker_exec(
+            container,
+            f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'",
+            timeout=15,
+        ).stdout
+        assert target_cov not in topics.split(), (
+            f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}"
+        )
+    finally:
+        stop_event.set()
+        publisher.join(timeout=2.0)
+        _terminate(node_proc)
+        server.shutdown()
diff --git a/tests/system/test_liveliness.py b/tests/system/test_liveliness.py
index 60ebea446..ce406444d 100644
--- a/tests/system/test_liveliness.py
+++ b/tests/system/test_liveliness.py
@@ -35,7 +35,9 @@
 
 # NatNet pose sentinel — only when LAUNCH_NATNET=true (robot natnet_ros2 + sim emulator).
 _NATNET_ENABLED = os.environ.get("LAUNCH_NATNET", "").strip().lower() in ("1", "true", "yes", "on")
-_NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone")
+# Relative pose topic from the robot's natnet_config.yaml profile (drone body), namespaced
+# per robot below. Each profile's drone uses this same leaf, so it holds for multi-robot too.
+_NATNET_POSE_TOPIC = os.environ.get("NATNET_POSE_TOPIC", "perception/optitrack/drone")
 _NATNET_MIN_HZ = 5.0
 # Cold Isaac boot: Pegasus load, Play, UDP connect — 60s was too tight vs other budgets.
 _NATNET_FIRST_MSG_TIMEOUT = 120
@@ -266,11 +268,10 @@ def ready():
     )
     @pytest.mark.dependency(depends=["sim_ready", "nodes"])
     def test_natnet_pose_alive(self, airstack_env):
-        """With LAUNCH_NATNET=true, /{robot}/perception/optitrack/{body}/pose_cov >= 5 Hz.
+        """With LAUNCH_NATNET=true, /{robot}/{natnet_pose_topic}/pose_cov >= 5 Hz.
 
-        In-sim counterpart to the host-side integration tests. Uses pose_cov
-        (always published) rather than bare <body> (gated on
-        publish_direct_optitrack).
+        In-sim counterpart to the host-side integration tests. Checks the pose_cov
+        variant of the drone body's configured topic (natnet_config.yaml profile).
         """
         cfg = airstack_env["cfg"]
         robot_containers = get_robot_containers(airstack_env["robot_pattern"])
@@ -280,7 +281,7 @@ def test_natnet_pose_alive(self, airstack_env):
         failures = []
         for n in range(1, airstack_env["num_robots"] + 1):
             container = robot_containers[n - 1]
-            topic = f"/robot_{n}/perception/optitrack/{_NATNET_BODY_NAME}/pose_cov"
+            topic = f"/robot_{n}/{_NATNET_POSE_TOPIC}/pose_cov"
             first = wait_for_first_message(
                 container,
                 topic,