├── LICENSE ├── README.md ├── example └── count_frames.py ├── natnet_client ├── __init__.py ├── data_descriptions.py ├── data_frame.py ├── data_types.py ├── event.py ├── nat_net_client.py ├── packet_buffer.py ├── packet_component.py ├── server_info.py └── version.py └── setup.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Tim Schneider 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Python NatNet Client 2 | 3 | Python client for Optitrack NatNet streams. 4 | 5 | ## Installation 6 | 7 | Install this package via pip: 8 | 9 | ```bash 10 | pip install git+https://github.com/TimSchneider42/python-natnet-client 11 | ``` 12 | 13 | ## Usage 14 | 15 | The following example highlights the basic usage of this package: 16 | 17 | ```python 18 | import time 19 | 20 | from natnet_client import DataDescriptions, DataFrame, NatNetClient 21 | 22 | 23 | def receive_new_frame(data_frame: DataFrame): 24 | global num_frames 25 | num_frames += 1 26 | 27 | 28 | def receive_new_desc(desc: DataDescriptions): 29 | print("Received data descriptions.") 30 | 31 | 32 | num_frames = 0 33 | if __name__ == "__main__": 34 | streaming_client = NatNetClient(server_ip_address="127.0.0.1", local_ip_address="127.0.0.1", use_multicast=False) 35 | streaming_client.on_data_description_received_event.handlers.append(receive_new_desc) 36 | streaming_client.on_data_frame_received_event.handlers.append(receive_new_frame) 37 | 38 | with streaming_client: 39 | streaming_client.request_modeldef() 40 | 41 | for i in range(10): 42 | time.sleep(1) 43 | streaming_client.update_sync() 44 | print(f"Received {num_frames} frames in {i + 1}s") 45 | ``` 46 | 47 | In this example, we first instantiate `NatNetClient` with the connection parameters and attach one callback function to 48 | each of its events. The `streaming_client.on_data_description_received_event` event is triggered whenever a new data 49 | description packet arrives, while the `streaming_client.on_data_frame_received_event` event is triggered on each 50 | incoming data frame. For the configuration of the NatNet server, please refer to the official documentation. 51 | 52 | You can process data synchronously, as in this example, by calling `streaming_client.update_sync()` in your run loop. 53 | Alternatively, you can call `streaming_client.run_async()` once after connecting, which will handle data asynchronously 54 | in two additional threads. 55 | 56 | We then use the `streaming_client` instance as a context manager, which is equivalent to 57 | calling `streaming_client.connect()` (and `streaming_client.shutdown()` afterwards). After the client has been 58 | connected, we request the model definitions from the server, which causes it to send a data description packet. Note 59 | that data frames do not have to be explicitly requested, but are continuously streamed once a connection has been 60 | established. 61 | 62 | Apart from requesting model definitions, the `NatNetClient` class allows sending arbitrary commands to the NatNet server 63 | via the `send_command` and `send_request` functions. For a list of different commands and requests, please refer to the 64 | official documentations. 65 | 66 | ## Notes 67 | 68 | As of Motive version 2.3, the marker positions of rigid bodies are only transmitted correctly if "Y-up" is selected in 69 | the streaming pane. If "Z-up" is selected, the frame of the rigid bodies is rotated but the marker positions are not, 70 | resulting in wrong positions of the markers relative to the rigid body. 71 | -------------------------------------------------------------------------------- /example/count_frames.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from natnet_client import DataDescriptions, DataFrame, NatNetClient 4 | 5 | 6 | def receive_new_frame(data_frame: DataFrame): 7 | global num_frames 8 | num_frames += 1 9 | 10 | 11 | def receive_new_desc(desc: DataDescriptions): 12 | print("Received data descriptions.") 13 | 14 | 15 | num_frames = 0 16 | if __name__ == "__main__": 17 | streaming_client = NatNetClient(server_ip_address="127.0.0.1", local_ip_address="127.0.0.1", use_multicast=False) 18 | streaming_client.on_data_description_received_event.handlers.append(receive_new_desc) 19 | streaming_client.on_data_frame_received_event.handlers.append(receive_new_frame) 20 | 21 | with streaming_client: 22 | streaming_client.request_modeldef() 23 | 24 | for i in range(10): 25 | time.sleep(1) 26 | streaming_client.update_sync() 27 | print(f"Received {num_frames} frames in {i + 1}s") 28 | -------------------------------------------------------------------------------- /natnet_client/__init__.py: -------------------------------------------------------------------------------- 1 | from .data_descriptions import * 2 | from .data_frame import * 3 | from .event import Event 4 | from .nat_net_client import NatNetClient 5 | from .server_info import * 6 | from .version import * 7 | -------------------------------------------------------------------------------- /natnet_client/data_descriptions.py: -------------------------------------------------------------------------------- 1 | from typing import NamedTuple, Tuple, Optional 2 | 3 | from .data_types import Vec3, Vec4 4 | from .packet_buffer import PacketBuffer 5 | from .packet_component import PacketComponent, PacketComponentArray 6 | from .version import Version 7 | 8 | 9 | class MarkerSetDescription(PacketComponent, NamedTuple( 10 | "MarkerSetDescriptionFields", 11 | (("name", str), 12 | ("marker_names", Tuple[str, ...])))): 13 | 14 | @classmethod 15 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "MarkerSetDescription": 16 | name = buffer.read_string() 17 | 18 | marker_count = buffer.read_uint32() 19 | marker_names = [buffer.read_string() for _ in range(marker_count)] 20 | return MarkerSetDescription(name, tuple(marker_names)) 21 | 22 | 23 | class RigidBodyMarkerDescription(PacketComponentArray, NamedTuple( 24 | "RigidBodyMarkerDescriptionFields", 25 | (("name", Optional[str]), 26 | ("active_label", int), 27 | ("pos", Vec3)))): 28 | 29 | @classmethod 30 | def read_array_from_buffer(cls, buffer: PacketBuffer, 31 | protocol_version: Version) -> "Tuple[RigidBodyMarkerDescription, ...]": 32 | marker_count = buffer.read_uint32() 33 | pos = [buffer.read_float32_array(3) for _ in range(marker_count)] 34 | active_labels = [buffer.read_uint32() for _ in range(marker_count)] 35 | names = [buffer.read_string() for _ in range(marker_count)] if protocol_version >= Version(4) else \ 36 | [None] * marker_count 37 | return tuple(RigidBodyMarkerDescription(*a) for a in zip(names, active_labels, pos)) 38 | 39 | 40 | class RigidBodyDescription(PacketComponent, NamedTuple( 41 | "RigidBodyDescriptionFields", 42 | (("name", Optional[str]), 43 | ("id_num", int), 44 | ("parent_id", int), 45 | ("pos", Vec3), 46 | ("markers", Tuple[RigidBodyMarkerDescription, ...])))): 47 | 48 | @classmethod 49 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "RigidBodyDescription": 50 | # Version 2.0 or higher 51 | name = buffer.read_string() if protocol_version >= Version(2) else None 52 | 53 | new_id = buffer.read_uint32() 54 | parent_id = buffer.read_uint32() 55 | pos = buffer.read_float32_array(3) 56 | 57 | # Version 3.0 and higher, rigid body marker information contained in description 58 | if protocol_version >= Version(3): 59 | marker_descriptions = RigidBodyMarkerDescription.read_array_from_buffer(buffer, protocol_version) 60 | else: 61 | marker_descriptions = [] 62 | 63 | return RigidBodyDescription(name, new_id, parent_id, pos, tuple(marker_descriptions)) 64 | 65 | 66 | class SkeletonDescription(PacketComponent, NamedTuple( 67 | "SkeletonDescriptionFields", 68 | (("name", str), 69 | ("id_num", int), 70 | ("rigid_body_descriptions", Tuple[RigidBodyDescription, ...])))): 71 | 72 | @classmethod 73 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "SkeletonDescription": 74 | name = buffer.read_string() 75 | new_id = buffer.read_uint32() 76 | rigid_body_count = buffer.read_uint32() 77 | 78 | # Loop over all Rigid Bodies 79 | rigid_body_descs = [ 80 | RigidBodyDescription.read_from_buffer(buffer, protocol_version) 81 | for _ in range(rigid_body_count)] 82 | return SkeletonDescription(name, new_id, tuple(rigid_body_descs)) 83 | 84 | 85 | class ForcePlateDescription(PacketComponent, NamedTuple( 86 | "ForcePlateDescriptionFields", 87 | (("id_num", int), 88 | ("serial_number", str), 89 | ("width", float), 90 | ("length", float), 91 | ("position", Vec3), 92 | ("cal_matrix", Tuple[Tuple[float, ...]]), 93 | ("corners", Tuple[Vec3, Vec3, Vec3, Vec3]), 94 | ("plate_type", int), 95 | ("channel_data_type", int), 96 | ("channel_list", Tuple[str, ...])))): 97 | 98 | @classmethod 99 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> Optional["ForcePlateDescription"]: 100 | if protocol_version >= Version(3): 101 | new_id = buffer.read_uint32() 102 | serial_number = buffer.read_string() 103 | width = buffer.read_float32() 104 | length = buffer.read_float32() 105 | origin = buffer.read_float32_array(3) 106 | 107 | cal_matrix_flat = buffer.read_float32_array(12 * 12) 108 | cal_matrix = [cal_matrix_flat[i * 12:(i + 1) * 12] for i in range(12)] 109 | 110 | corners_flat = buffer.read_float32_array(3 * 3) 111 | corners = [corners_flat[i * 3:(i + 1) * 3] for i in range(3)] 112 | 113 | plate_type = buffer.read_uint32() 114 | channel_data_type = buffer.read_uint32() 115 | num_channels = buffer.read_uint32() 116 | 117 | channels = [buffer.read_string() for _ in range(num_channels)] 118 | 119 | return ForcePlateDescription( 120 | new_id, serial_number, width, length, origin, tuple(cal_matrix), tuple(corners), plate_type, 121 | channel_data_type, tuple(channels)) 122 | else: 123 | return None 124 | 125 | 126 | class DeviceDescription(PacketComponent, NamedTuple( 127 | "DeviceDescriptionFields", 128 | (("id_num", int), 129 | ("name", str), 130 | ("serial_number", str), 131 | ("device_type", int), 132 | ("channel_data_type", int), 133 | ("channel_names", Tuple[str, ...])))): 134 | 135 | @classmethod 136 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> Optional["DeviceDescription"]: 137 | if protocol_version >= Version(3): 138 | new_id = buffer.read_uint32() 139 | name = buffer.read_string() 140 | serial_number = buffer.read_string() 141 | device_type = buffer.read_uint32() 142 | channel_data_type = buffer.read_uint32() 143 | num_channels = buffer.read_uint32() 144 | 145 | # Channel Names list of NoC strings 146 | channel_names = [buffer.read_string() for _ in range(num_channels)] 147 | 148 | return DeviceDescription(new_id, name, serial_number, device_type, channel_data_type, 149 | tuple(channel_names)) 150 | else: 151 | return None 152 | 153 | 154 | class CameraDescription(PacketComponent, NamedTuple( 155 | "CameraDescriptionFields", 156 | (("name", str), 157 | ("position", Vec3), 158 | ("orientation_quat", Vec4)))): 159 | 160 | @classmethod 161 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "CameraDescription": 162 | name = buffer.read_string() 163 | position = buffer.read_float32_array(3) 164 | orientation = buffer.read_float32_array(4) 165 | 166 | return CameraDescription(name, position, orientation) 167 | 168 | 169 | class DataDescriptions(PacketComponent, NamedTuple("DataDescriptions", ( 170 | ("marker_sets", Tuple[MarkerSetDescription, ...]), 171 | ("rigid_bodies", Tuple[RigidBodyDescription, ...]), 172 | ("skeletons", Tuple[SkeletonDescription, ...]), 173 | ("force_plates", Tuple[ForcePlateDescription, ...]), 174 | ("devices", Tuple[DeviceDescription, ...]), 175 | ("cameras", Tuple[CameraDescription, ...])))): 176 | 177 | @classmethod 178 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "DataDescriptions": 179 | data_desc_types = { 180 | 0: ("marker_sets", MarkerSetDescription), 181 | 1: ("rigid_bodies", RigidBodyDescription), 182 | 2: ("skeletons", SkeletonDescription), 183 | 3: ("force_plates", ForcePlateDescription), 184 | 4: ("devices", DeviceDescription), 185 | 5: ("cameras", CameraDescription) 186 | } 187 | data_dict = {n: [] for i, (n, f) in data_desc_types.items()} 188 | # # of data sets to process 189 | dataset_count = buffer.read_uint32() 190 | for i in range(0, dataset_count): 191 | data_type = buffer.read_uint32() 192 | if data_type in data_desc_types: 193 | name, desc_type = data_desc_types[data_type] 194 | unpacked_data = desc_type.read_from_buffer(buffer, protocol_version) 195 | data_dict[name].append(unpacked_data) 196 | else: 197 | print(f"Type: {data_type} unknown. Stopped processing at {buffer.pointer}/{len(buffer.data)} bytes " 198 | f"({i + 1}/{dataset_count}) datasets.") 199 | return DataDescriptions(**data_dict) 200 | -------------------------------------------------------------------------------- /natnet_client/data_frame.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, fields 2 | from inspect import isclass 3 | from typing import Tuple, Optional 4 | 5 | from .data_types import Vec3, Vec4 6 | from .packet_buffer import PacketBuffer 7 | from .packet_component import PacketComponent 8 | from .version import Version 9 | 10 | 11 | @dataclass(frozen=True) 12 | class FramePrefix(PacketComponent): 13 | frame_number: int 14 | 15 | @classmethod 16 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "FramePrefix": 17 | return FramePrefix(buffer.read_uint32()) 18 | 19 | 20 | @dataclass(frozen=True) 21 | class MarkerSet(PacketComponent): 22 | model_name: str 23 | marker_pos_list: Tuple[Vec3, ...] 24 | 25 | @classmethod 26 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "MarkerSet": 27 | model_name = buffer.read_string() 28 | marker_count = buffer.read_uint32() 29 | marker_pos_list = [buffer.read_float32_array(3) for _ in range(marker_count)] 30 | return MarkerSet(model_name, tuple(marker_pos_list)) 31 | 32 | 33 | # Only used up until version 3.0, where this information has been moved to the description 34 | @dataclass(frozen=True) 35 | class RigidBodyMarker: 36 | pos: Vec3 37 | id_num: Optional[int] 38 | size: Optional[int] 39 | 40 | 41 | @dataclass(frozen=True) 42 | class RigidBody(PacketComponent): 43 | id_num: int 44 | pos: Vec3 45 | rot: Vec4 46 | markers: Optional[Tuple[RigidBodyMarker, ...]] 47 | tracking_valid: Optional[bool] 48 | marker_error: Optional[float] 49 | 50 | @classmethod 51 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "RigidBody": 52 | id_num = buffer.read_uint32() 53 | pos = buffer.read_float32_array(3) 54 | rot = buffer.read_float32_array(4) 55 | # RB Marker Data ( Before version 3.0. After Version 3.0 Marker data is in description ) 56 | if protocol_version < Version(3, 0): 57 | marker_count = buffer.read_uint32() 58 | marker_positions = [buffer.read_float32_array(3) for _ in range(marker_count)] 59 | 60 | if protocol_version >= Version(2): 61 | marker_ids = [buffer.read_uint32() for _ in range(marker_count)] 62 | marker_sizes = [buffer.read_float32() for _ in range(marker_count)] 63 | else: 64 | marker_ids = marker_sizes = [None for _ in range(marker_count)] 65 | 66 | markers = [RigidBodyMarker(*f) for f in zip(marker_positions, marker_ids, marker_sizes)] 67 | else: 68 | markers = None 69 | if protocol_version >= Version(2): 70 | marker_error = buffer.read_float32() 71 | else: 72 | marker_error = None 73 | 74 | # Version 2.6 and later 75 | if protocol_version >= Version(2, 6): 76 | param = buffer.read_uint16() 77 | tracking_valid = (param & 0x01) != 0 78 | else: 79 | tracking_valid = None 80 | 81 | return RigidBody(id_num, pos, rot, markers, tracking_valid, marker_error) 82 | 83 | 84 | @dataclass(frozen=True) 85 | class Skeleton(PacketComponent): 86 | id_num: int 87 | rigid_bodies: Tuple[RigidBody, ...] 88 | 89 | @classmethod 90 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "Skeleton": 91 | id_num = buffer.read_uint32() 92 | rigid_body_count = buffer.read_uint32() 93 | rigid_bodies = [RigidBody.read_from_buffer(buffer, protocol_version) for _ in range(rigid_body_count)] 94 | 95 | return Skeleton(id_num, tuple(rigid_bodies)) 96 | 97 | 98 | @dataclass(frozen=True) 99 | class LabeledMarker(PacketComponent): 100 | id_num: int 101 | pos: Vec3 102 | size: int 103 | param: Optional[int] 104 | residual: Optional[float] 105 | 106 | @classmethod 107 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "LabeledMarker": 108 | id_num = buffer.read_uint32() 109 | pos = buffer.read_float32_array(3) 110 | size = buffer.read_float32() 111 | 112 | # Version 2.6 and later 113 | if protocol_version >= Version(2, 6): 114 | param = buffer.read_uint16() 115 | else: 116 | param = None 117 | 118 | # Version 3.0 and later 119 | if protocol_version >= Version(3): 120 | residual = buffer.read_float32() 121 | else: 122 | residual = None 123 | 124 | return cls(id_num, pos, size, param, residual) 125 | 126 | @property 127 | def model_id(self): 128 | return self.id_num >> 16 129 | 130 | @property 131 | def marker_id(self): 132 | return self.id_num & 0x0000ffff 133 | 134 | @property 135 | def occluded(self): 136 | return bool(self.param & 0x01) 137 | 138 | @property 139 | def point_cloud_solved(self): 140 | return bool(self.param & 0x02) 141 | 142 | @property 143 | def model_solved(self): 144 | return bool(self.param & 0x04) 145 | 146 | @property 147 | def has_model(self): 148 | return bool(self.param & 0x08) 149 | 150 | @property 151 | def unlabeled(self): 152 | return bool(self.param & 0x10) 153 | 154 | @property 155 | def active(self): 156 | return bool(self.param & 0x20) 157 | 158 | 159 | @dataclass(frozen=True) 160 | class ForcePlate(PacketComponent): 161 | id_num: int 162 | channel_arrays: Tuple[Tuple[float, ...], ...] 163 | 164 | @classmethod 165 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "ForcePlate": 166 | id_num = buffer.read_uint32() 167 | channel_count = buffer.read_uint32() 168 | channel_arrays = tuple(buffer.read_float32_array(buffer.read_uint32()) for _ in range(channel_count)) 169 | return ForcePlate(id_num, channel_arrays) 170 | 171 | 172 | @dataclass(frozen=True) 173 | class Device(PacketComponent): 174 | id_num: int 175 | channel_arrays: Tuple[Tuple[float, ...], ...] 176 | 177 | @classmethod 178 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "Device": 179 | id_num = buffer.read_uint32() 180 | channel_count = buffer.read_uint32() 181 | channel_arrays = tuple(buffer.read_float32_array(buffer.read_uint32()) for _ in range(channel_count)) 182 | return Device(id_num, channel_arrays) 183 | 184 | 185 | @dataclass(frozen=True) 186 | class FrameSuffix(PacketComponent): 187 | timecode: int 188 | timecode_sub: int 189 | timestamp: float 190 | stamp_camera_mid_exposure: Optional[int] 191 | stamp_data_received: Optional[int] 192 | stamp_transmit: Optional[int] 193 | param: int 194 | is_recording: bool 195 | tracked_models_changed: bool 196 | 197 | @classmethod 198 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "FrameSuffix": 199 | # Timecode 200 | timecode = buffer.read_uint32() 201 | timecode_sub = buffer.read_uint32() 202 | 203 | # Timestamp (increased to double precision in 2.7 and later) 204 | if protocol_version >= Version(2, 7): 205 | timestamp = buffer.read_float64() 206 | else: 207 | timestamp = buffer.read_float32() 208 | 209 | # Hires Timestamp (Version 3.0 and later) 210 | if protocol_version >= Version(3): 211 | stamp_camera_mid_exposure = buffer.read_uint64() 212 | stamp_data_received = buffer.read_uint64() 213 | stamp_transmit = buffer.read_uint64() 214 | else: 215 | stamp_camera_mid_exposure = stamp_data_received = stamp_transmit = None 216 | 217 | # Frame parameters 218 | param = buffer.read_uint16() 219 | is_recording = (param & 0x01) != 0 220 | tracked_models_changed = (param & 0x02) != 0 221 | 222 | return FrameSuffix(timecode, timecode_sub, timestamp, stamp_camera_mid_exposure, stamp_data_received, 223 | stamp_transmit, param, is_recording, tracked_models_changed) 224 | 225 | 226 | @dataclass(frozen=True) 227 | class DataFrame(PacketComponent): 228 | prefix: FramePrefix 229 | marker_sets: Tuple[MarkerSet, ...] 230 | unlabeled_marker_pos: Tuple[Vec3, ...] 231 | rigid_bodies: Tuple[RigidBody, ...] 232 | skeletons: Tuple[Skeleton, ...] 233 | labeled_markers: Tuple[LabeledMarker, ...] 234 | force_plates: Tuple[ForcePlate, ...] 235 | devices: Tuple[Device, ...] 236 | suffix: FrameSuffix 237 | 238 | MIN_VERSIONS = { 239 | "prefix": Version(0), 240 | "marker_sets": Version(0), 241 | "unlabeled_marker_pos": Version(0), 242 | "rigid_bodies": Version(0), 243 | "skeletons": Version(2, 1), 244 | "labeled_markers": Version(2, 3), 245 | "force_plates": Version(2, 9), 246 | "devices": Version(2, 11), 247 | "suffix": Version(0) 248 | } 249 | 250 | @classmethod 251 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "DataFrame": 252 | kwargs = {} 253 | 254 | for field in fields(cls): 255 | if protocol_version >= cls.MIN_VERSIONS[field.name]: 256 | if isclass(field.type) and issubclass(field.type, PacketComponent): 257 | kwargs[field.name] = field.type.read_from_buffer(buffer, protocol_version) 258 | else: 259 | # Type is a tuple 260 | element_count = buffer.read_uint32() 261 | generic_type = field.type.__args__[0] 262 | if generic_type == Vec3: 263 | kwargs[field.name] = tuple(buffer.read_float32_array(3) for _ in range(element_count)) 264 | else: 265 | kwargs[field.name] = tuple( 266 | generic_type.read_from_buffer(buffer, protocol_version) for _ in range(element_count)) 267 | else: 268 | kwargs[field.name] = None 269 | 270 | return cls(**kwargs) 271 | -------------------------------------------------------------------------------- /natnet_client/data_types.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | 3 | Vec3 = Tuple[float, float, float] 4 | Vec4 = Tuple[float, float, float, float] 5 | -------------------------------------------------------------------------------- /natnet_client/event.py: -------------------------------------------------------------------------------- 1 | class Event: 2 | def __init__(self): 3 | self.handlers = [] 4 | 5 | def call(self, *args, **kwargs): 6 | for h in self.handlers: 7 | h(*args, **kwargs) 8 | 9 | def __call__(self, *args, **kwargs): 10 | self.call(*args, **kwargs) 11 | -------------------------------------------------------------------------------- /natnet_client/nat_net_client.py: -------------------------------------------------------------------------------- 1 | import socket 2 | from threading import Thread 3 | import time 4 | from typing import Optional 5 | 6 | from .data_frame import DataFrame 7 | from .event import Event 8 | from .server_info import ServerInfo 9 | from .data_descriptions import DataDescriptions 10 | from .packet_buffer import PacketBuffer 11 | from .version import Version 12 | 13 | 14 | class NatNetError(Exception): 15 | pass 16 | 17 | 18 | class NatNetNetworkError(NatNetError): 19 | def __init__(self, socket_name: str, multicast: bool, inner_error: socket.error): 20 | super().__init__( 21 | f"{socket_name} socket error occurred:\n{inner_error}\nCheck Motive/Server mode requested mode agreement " 22 | f"(you requested {'multi' if multicast else 'uni'}cast).") 23 | self.inner_error = inner_error 24 | 25 | 26 | class NatNetProtocolError(NatNetError): 27 | pass 28 | 29 | 30 | class NatNetClient: 31 | # Client/server message ids 32 | NAT_CONNECT = 0 33 | NAT_SERVERINFO = 1 34 | NAT_REQUEST = 2 35 | NAT_RESPONSE = 3 36 | NAT_REQUEST_MODELDEF = 4 37 | NAT_MODELDEF = 5 38 | NAT_REQUEST_FRAMEOFDATA = 6 39 | NAT_FRAMEOFDATA = 7 40 | NAT_MESSAGESTRING = 8 41 | NAT_DISCONNECT = 9 42 | NAT_KEEPALIVE = 10 43 | NAT_UNRECOGNIZED_REQUEST = 100 44 | 45 | def __init__(self, server_ip_address: str = "127.0.0.1", local_ip_address: str = "127.0.0.1", 46 | multicast_address: str = "239.255.42.99", command_port: int = 1510, data_port: int = 1511, 47 | use_multicast: bool = True): 48 | self.__server_ip_address = server_ip_address 49 | self.__local_ip_address = local_ip_address 50 | self.__multicast_address = multicast_address 51 | self.__command_port = command_port 52 | self.__data_port = data_port 53 | self.__use_multicast = use_multicast 54 | 55 | self.__server_info = None 56 | 57 | # NatNet stream version. This will be updated to the actual version the server is using during runtime. 58 | self.__current_protocol_version: Optional[Version] = None 59 | 60 | self.__command_thread = None 61 | self.__data_thread = None 62 | self.__command_socket: Optional[socket.socket] = None 63 | self.__data_socket: Optional[socket.socket] = None 64 | 65 | self.__stop_threads = False 66 | 67 | self.__on_data_frame_received_event = Event() 68 | self.__on_data_description_received_event = Event() 69 | 70 | @property 71 | def connected(self): 72 | return self.__command_socket is not None and self.__data_socket is not None and self.__server_info is not None 73 | 74 | @staticmethod 75 | def __create_socket(addr: str = "", port: int = 0): 76 | # Create a command socket to attach to the NatNet stream 77 | result = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 78 | # allow multiple clients on same machine to use multicast group address/port 79 | result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 80 | try: 81 | result.bind((addr, port)) 82 | except: 83 | result.close() 84 | raise 85 | result.settimeout(0.0) 86 | return result 87 | 88 | def __create_command_socket(self): 89 | if self.__use_multicast: 90 | addr = "" 91 | else: 92 | addr = self.__local_ip_address 93 | try: 94 | result = self.__create_socket(addr, 0) 95 | except socket.error as ex: 96 | raise NatNetNetworkError("Command", self.__use_multicast, ex) 97 | 98 | if self.__use_multicast: 99 | # set to broadcast mode 100 | result.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) 101 | return result 102 | 103 | # Create a data socket to attach to the NatNet stream 104 | def __create_data_socket(self, port): 105 | if self.__use_multicast: 106 | addr = self.__multicast_address 107 | else: 108 | addr = "" 109 | port = 0 110 | try: 111 | result = self.__create_socket(addr, port) 112 | except socket.error as ex: 113 | raise NatNetNetworkError("Data", self.__use_multicast, ex) 114 | 115 | if self.__use_multicast: 116 | result.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, 117 | socket.inet_aton(self.__multicast_address) + socket.inet_aton(self.__local_ip_address)) 118 | return result 119 | 120 | def __socket_thread_func(self, in_socket: socket.socket, recv_buffer_size: int = 64 * 1024, 121 | send_keep_alive: bool = False): 122 | while not self.__stop_threads: 123 | # Use timeout to ensure that thread can terminate 124 | self.__process_socket(in_socket, recv_buffer_size=recv_buffer_size, send_keep_alive=send_keep_alive) 125 | 126 | def __process_socket(self, in_socket: socket.socket, recv_buffer_size: int = 64 * 1024, 127 | send_keep_alive: bool = False): 128 | if send_keep_alive: 129 | self.send_request(self.NAT_KEEPALIVE) 130 | try: 131 | data, addr = in_socket.recvfrom(recv_buffer_size) 132 | if len(data) > 0: 133 | self.__process_message(PacketBuffer(data)) 134 | return True 135 | except (BlockingIOError, socket.timeout): 136 | pass 137 | return False 138 | 139 | def __process_message(self, buffer: PacketBuffer): 140 | message_id = buffer.read_uint16() 141 | packet_size = buffer.read_uint16() 142 | if len(buffer.data) - 4 != packet_size: 143 | print(f"Warning: actual packet size ({len(buffer.data) - 4}) not consistent with packet size in the " 144 | f"header ({packet_size})") 145 | if message_id == self.NAT_SERVERINFO: 146 | self.__server_info = ServerInfo.read_from_buffer(buffer, self.__current_protocol_version) 147 | self.__current_protocol_version = self.__server_info.nat_net_protocol_version 148 | else: 149 | if self.__current_protocol_version is None: 150 | print(f"Warning: dropping packet of type {message_id} as server info has not been received yet.") 151 | if message_id == self.NAT_FRAMEOFDATA: 152 | data_frame = DataFrame.read_from_buffer(buffer, self.__current_protocol_version) 153 | self.__on_data_frame_received_event.call(data_frame) 154 | elif message_id == self.NAT_MODELDEF: 155 | data_descs = DataDescriptions.read_from_buffer(buffer, self.__current_protocol_version) 156 | self.__on_data_description_received_event.call(data_descs) 157 | def send_request(self, command: int, command_str: str = ""): 158 | if command in [self.NAT_REQUEST_MODELDEF, self.NAT_REQUEST_FRAMEOFDATA, self.NAT_KEEPALIVE]: 159 | command_str = "" 160 | if command == self.NAT_CONNECT: 161 | command_str = "Ping" 162 | packet_size = len(command_str) + 1 163 | 164 | data = command.to_bytes(2, byteorder="little") 165 | data += packet_size.to_bytes(2, byteorder="little") 166 | 167 | data += command_str.encode("utf-8") 168 | data += b"\0" 169 | 170 | return self.__command_socket.sendto(data, (self.__server_ip_address, self.__command_port)) 171 | 172 | def request_modeldef(self): 173 | self.send_request(self.NAT_REQUEST_MODELDEF) 174 | 175 | def send_command(self, command_str: str): 176 | if not self.connected: 177 | raise NatNetError("NatNet client is not connected to a server.") 178 | return self.send_request(self.NAT_REQUEST, command_str) 179 | 180 | def connect(self, timeout: float = 5.0): 181 | if not self.connected: 182 | self.__data_socket = self.__create_data_socket(self.__data_port) 183 | self.__command_socket = self.__create_command_socket() 184 | 185 | # Get NatNet and server versions 186 | self.send_request(self.NAT_CONNECT) 187 | 188 | start_time = time.time() 189 | while self.__server_info is None: 190 | # Waiting for reply from server 191 | self.__process_socket(self.__command_socket, send_keep_alive=not self.__use_multicast) 192 | if self.__server_info is None and (time.time() - start_time) >= timeout: 193 | self.shutdown() 194 | raise TimeoutError() 195 | time.sleep(0.1) 196 | 197 | def run_async(self): 198 | if not self.running_asynchronously: 199 | self.__stop_threads = False 200 | 201 | # To ensure that threads can terminate 202 | self.__data_socket.settimeout(0.1) 203 | self.__command_socket.settimeout(0.1) 204 | 205 | # Create a separate thread for receiving data packets 206 | self.__data_thread = Thread(target=self.__socket_thread_func, args=(self.__data_socket,)) 207 | self.__data_thread.start() 208 | 209 | # Create a separate thread for receiving command packets 210 | self.__command_thread = Thread( 211 | target=self.__socket_thread_func, args=(self.__command_socket,), 212 | kwargs={"send_keep_alive": not self.__use_multicast}) 213 | self.__command_thread.start() 214 | 215 | def stop_async(self): 216 | if self.running_asynchronously: 217 | self.__stop_threads = True 218 | if self.__command_thread is not None: 219 | self.__command_thread.join() 220 | if self.__data_thread is not None: 221 | self.__data_thread.join() 222 | self.__command_thread = self.__data_thread = None 223 | self.__data_socket.settimeout(0.0) 224 | self.__command_socket.settimeout(0.0) 225 | 226 | def update_sync(self): 227 | assert not self.running_asynchronously, "Cannot update synchronously while running asynchronously." 228 | while self.__process_socket(self.__data_socket): 229 | pass 230 | while self.__process_socket(self.__command_socket, send_keep_alive=not self.__use_multicast): 231 | pass 232 | 233 | def shutdown(self): 234 | self.stop_async() 235 | if self.__command_socket is not None: 236 | self.__command_socket.close() 237 | if self.__data_socket is not None: 238 | self.__data_socket.close() 239 | self.__command_socket = self.__data_socket = self.__server_info = None 240 | 241 | def __enter__(self): 242 | self.connect() 243 | return self 244 | 245 | def __exit__(self, exc_type, exc_val, exc_tb): 246 | self.shutdown() 247 | 248 | @property 249 | def on_data_frame_received_event(self) -> Event: 250 | return self.__on_data_frame_received_event 251 | 252 | @property 253 | def on_data_description_received_event(self) -> Event: 254 | return self.__on_data_description_received_event 255 | 256 | @property 257 | def server_info(self) -> Optional[ServerInfo]: 258 | return self.__server_info 259 | 260 | @property 261 | def can_change_protocol_version(self) -> bool: 262 | if self.__server_info is not None: 263 | return self.__server_info.nat_net_protocol_version >= Version(4) and not self.__use_multicast 264 | return False 265 | 266 | @property 267 | def protocol_version(self) -> Optional[Version]: 268 | return self.__current_protocol_version 269 | 270 | @protocol_version.setter 271 | def protocol_version(self, desired_version: Version): 272 | if not self.can_change_protocol_version: 273 | raise NatNetProtocolError("Server does not support changing the NatNet protocol version.") 274 | desired_version = desired_version.truncate(2) 275 | if self.can_change_protocol_version and desired_version != self.__current_protocol_version.truncate(2): 276 | sz_command = f"Bitstream,{desired_version}" 277 | return_code = self.send_command(sz_command) 278 | if return_code >= 0: 279 | self.__current_protocol_version = desired_version 280 | self.send_command("TimelinePlay") 281 | time.sleep(0.1) 282 | for cmd in ["TimelinePlay", "TimelineStop", "SetPlaybackCurrentFrame,0", "TimelineStop"]: 283 | self.send_command(cmd) 284 | time.sleep(2) 285 | else: 286 | raise NatNetProtocolError("Failed to set NatNet protocol version") 287 | 288 | @property 289 | def server_ip_address(self) -> str: 290 | return self.__server_ip_address 291 | 292 | @property 293 | def local_ip_address(self) -> str: 294 | return self.__local_ip_address 295 | 296 | @property 297 | def multicast_address(self) -> str: 298 | return self.__multicast_address 299 | 300 | @property 301 | def command_port(self) -> int: 302 | return self.__command_port 303 | 304 | @property 305 | def data_port(self) -> int: 306 | return self.__data_port 307 | 308 | @property 309 | def use_multicast(self) -> bool: 310 | return self.__use_multicast 311 | 312 | @property 313 | def running_asynchronously(self): 314 | return self.__command_thread is not None or self.__data_thread is not None 315 | -------------------------------------------------------------------------------- /natnet_client/packet_buffer.py: -------------------------------------------------------------------------------- 1 | import struct 2 | from typing import Optional, Union, Tuple, Any 3 | 4 | 5 | class PacketBuffer: 6 | def __init__(self, data: bytes): 7 | self.__data = memoryview(data) # Using a memoryview here ensures that slices do not create copies 8 | self.pointer = 0 9 | 10 | @property 11 | def data(self): 12 | return self.__data 13 | 14 | def read_string(self, max_length: Optional[int] = None, static_length: bool = False) -> str: 15 | if max_length is None: 16 | data_slice = self.__data[self.pointer:] 17 | else: 18 | data_slice = self.__data[self.pointer:self.pointer + max_length] 19 | str_enc, separator, remainder = bytes(data_slice).partition(b"\0") 20 | str_dec = str_enc.decode("utf-8") 21 | if static_length: 22 | assert max_length is not None 23 | self.pointer += max_length 24 | else: 25 | self.pointer += len(str_enc) + 1 26 | return str_dec 27 | 28 | def read(self, data_type: Union[struct.Struct, str]) -> Tuple[Any, ...]: 29 | if isinstance(data_type, str): 30 | data_type = struct.Struct(data_type) 31 | values = data_type.unpack_from(self.__data, offset=self.pointer) 32 | self.pointer += data_type.size 33 | return values 34 | 35 | def read_uint16(self) -> int: 36 | return self.read("H")[0] 37 | 38 | def read_uint32(self) -> int: 39 | return self.read("I")[0] 40 | 41 | def read_uint64(self) -> int: 42 | return self.read("L")[0] 43 | 44 | def read_float32(self) -> float: 45 | return self.read("f")[0] 46 | 47 | def read_float32_array(self, count: int) -> Tuple[float, ...]: 48 | return self.read("f" * count) 49 | 50 | def read_float64(self) -> float: 51 | return self.read("d")[0] 52 | -------------------------------------------------------------------------------- /natnet_client/packet_component.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | 3 | from typing import Tuple 4 | 5 | from .packet_buffer import PacketBuffer 6 | from .version import Version 7 | 8 | 9 | class PacketComponent(ABC): 10 | @classmethod 11 | @abstractmethod 12 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "PacketComponent": 13 | pass 14 | 15 | 16 | class PacketComponentArray(ABC): 17 | @classmethod 18 | @abstractmethod 19 | def read_array_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "Tuple[PacketComponent]": 20 | pass 21 | -------------------------------------------------------------------------------- /natnet_client/server_info.py: -------------------------------------------------------------------------------- 1 | from typing import NamedTuple 2 | 3 | from .packet_buffer import PacketBuffer 4 | from .packet_component import PacketComponent 5 | from .version import Version 6 | 7 | 8 | class ServerInfo(PacketComponent, NamedTuple("ServerInfoFields", ( 9 | ("application_name", str), 10 | ("server_version", Version), 11 | ("nat_net_protocol_version", Version)))): 12 | 13 | @classmethod 14 | def read_from_buffer(cls, buffer: PacketBuffer, protocol_version: Version) -> "ServerInfo": 15 | application_name = buffer.read_string(256, static_length=True) 16 | server_version = Version(*buffer.read("BBBB")) 17 | nat_net_protocol_version = Version(*buffer.read("BBBB")) 18 | return cls(application_name, server_version, nat_net_protocol_version) 19 | -------------------------------------------------------------------------------- /natnet_client/version.py: -------------------------------------------------------------------------------- 1 | class Version: 2 | def __init__(self, *components: int): 3 | self.__components = tuple(components) 4 | 5 | @property 6 | def major(self): 7 | return self.__components[0] if len(self.__components) > 0 else 0 8 | 9 | @property 10 | def minor(self): 11 | return self.__components[1] if len(self.__components) > 1 else 0 12 | 13 | @property 14 | def revision(self): 15 | return self.__components[2] if len(self.__components) > 2 else 0 16 | 17 | @property 18 | def build(self): 19 | return self.__components[3] if len(self.__components) > 3 else 0 20 | 21 | def truncate(self, pos: int): 22 | return Version(*self.__components[:pos]) 23 | 24 | def __str__(self): 25 | return ".".join(map(str, self.__components)) 26 | 27 | __repr__ = __str__ 28 | 29 | @staticmethod 30 | def __compare(v1: "Version", v2: "Version") -> int: 31 | c1 = v1.__components 32 | c2 = v2.__components 33 | # Zero pad 34 | if len(c1) < len(c2): 35 | c1 += (0,) * (len(c2) - len(c1)) 36 | elif len(c2) < len(c1): 37 | c2 += (0,) * (len(c1) - len(c2)) 38 | for c1, c2 in zip(v1.__components, v2.__components): 39 | if c1 > c2: 40 | return 1 41 | elif c1 < c2: 42 | return -1 43 | return 0 44 | 45 | def __gt__(self, other: "Version"): 46 | if isinstance(other, Version): 47 | return self.__compare(self, other) == 1 48 | else: 49 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 50 | 51 | def __ge__(self, other: "Version"): 52 | if isinstance(other, Version): 53 | return self.__compare(self, other) in [0, 1] 54 | else: 55 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 56 | 57 | def __lt__(self, other: "Version"): 58 | if isinstance(other, Version): 59 | return self.__compare(self, other) == -1 60 | else: 61 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 62 | 63 | def __le__(self, other: "Version"): 64 | if isinstance(other, Version): 65 | return self.__compare(self, other) in [-1, 0] 66 | else: 67 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 68 | 69 | def __eq__(self, other: "Version"): 70 | if isinstance(other, Version): 71 | return self.__compare(self, other) == 0 72 | else: 73 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 74 | 75 | def __ne__(self, other: "Version"): 76 | if isinstance(other, Version): 77 | return self.__compare(self, other) != 0 78 | else: 79 | raise TypeError(f"Expected other to have type {Version}, got {type(other)}") 80 | 81 | @property 82 | def components(self): 83 | return self.__components 84 | 85 | @classmethod 86 | def from_str(cls, version_string: str): 87 | return cls(*map(int, version_string.split("."))) 88 | 89 | @classmethod 90 | def create(cls, major: int = 0, minor: int = 0, revision: int = 0, build: int = 0): 91 | return cls(major, minor, revision, build) 92 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r", encoding="utf-8") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name="natnet-client", 8 | version="0.1.1", 9 | author="Tim Schneider", 10 | author_email="schneider@ias.informatik.tu-darmstadt.de", 11 | description="Python client for Optitrack NatNet streams.", 12 | long_description=long_description, 13 | long_description_content_type="text/markdown", 14 | url="https://github.com/TimSchneider42/python-natnet-client", 15 | project_urls={ 16 | "Bug Tracker": "https://github.com/TimSchneider42/python-natnet-client/issues", 17 | }, 18 | classifiers=[ 19 | "Programming Language :: Python :: 3", 20 | "License :: OSI Approved :: MIT License", 21 | "Operating System :: OS Independent", 22 | ], 23 | packages=["natnet_client"], 24 | python_requires=">=3.6", 25 | ) --------------------------------------------------------------------------------