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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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