poseproxy 0.0.1__tar.gz

poseproxy-0.0.1/.gitignore

@@ -0,0 +1,17 @@
+build
+*.o
+*.o.cmd
+.vscode/settings.json
+venv
+ablation_data
+*.tar.gz
+
+ablation_checkpoints
+ablation_data*
+.venv
+*.out
+*.wasm
+.idea
+cmake-build*
+*.o.*
+**/__pycache__

poseproxy-0.0.1/PKG-INFO

@@ -0,0 +1,78 @@
+Metadata-Version: 2.5
+Name: poseproxy
+Version: 0.0.1
+Summary: Simple Pose Forwarder For Motion Capturing Systems
+Author-email: Jonas Eschmann <jonas.eschmann@gmail.com>
+License: MIT
+Requires-Python: >=3.10
+Requires-Dist: websockets<16,>=15.0.1
+Provides-Extra: vicon
+Requires-Dist: pyvicon-datastream==0.2.4; extra == 'vicon'
+Description-Content-Type: text/markdown
+
+# Pose WebSocket server
+
+Streams one rigid body from any mocap system to a browser. Requires Python 3.10+.
+
+Run from the repository root:
+
+```sh
+python3 -m venv .venv
+. .venv/bin/activate
+pip install ./pose
+pose --demo
+```
+
+For Vicon:
+
+```sh
+pip install './pose[vicon]'
+pose.vicon --host 192.154.4.124 --list  # List current object names
+pose.vicon --host 192.154.4.124 --subject crazyflie | pose
+```
+
+Vicon uses the root segment; override with `--segment NAME`. Server defaults:
+`127.0.0.1:8765`; change with `--host`/`--port`. Stop with Ctrl-C.
+
+## Pose API
+
+Connect to `ws://127.0.0.1:8765/pose`. Each text message contains:
+
+```json
+{"timestamp":1789600000.125,"position":[0.1,-0.2,0.5],"quaternion":[0,0,0,1]}
+```
+
+- `timestamp`: Unix seconds when the producer received the sample.
+- `position`: meters, right-handed world frame: X forward, Y left, Z up.
+- `quaternion`: normalized `[x,y,z,w]`, rotating body-local vectors into world coordinates.
+
+All values must be finite numbers. The Vicon adapter converts millimeters to meters
+and reorders its SDK quaternion to this shared convention.
+
+The stream is read-only. Invalid/occluded poses are skipped; tracking loss means
+silence. Clients should detect stale data. Slow clients skip pending samples;
+new connections receive only new samples. Input EOF stops the server.
+
+Other adapters must emit this format as flushed, newline-delimited JSON to
+stdout, with diagnostics on stderr:
+
+```sh
+python -u my_mocap_adapter.py | pose
+```
+
+## Browser
+
+Run Python on the browser's computer. From a Connect button handler:
+
+```js
+const response = await fetch("http://127.0.0.1:8765/health");
+if (!response.ok) throw new Error("Pose server unavailable");
+const socket = new WebSocket("ws://127.0.0.1:8765/pose");
+socket.onmessage = ({data}) => console.log(JSON.parse(data));
+```
+
+Grant Chrome's local/loopback permission. Allowed origins: `https://rc.rl.tools`,
+`http://localhost:8000`, `http://127.0.0.1:8000`; extend with `--allow-origin URL`.
+`/health` returns `last_pose_age_s` (`null` before any sample).
+
+Tests: `python -B -m unittest discover -s pose/tests -v`.

poseproxy-0.0.1/README.md

@@ -0,0 +1,66 @@
+# Pose WebSocket server
+
+Streams one rigid body from any mocap system to a browser. Requires Python 3.10+.
+
+Run from the repository root:
+
+```sh
+python3 -m venv .venv
+. .venv/bin/activate
+pip install ./pose
+pose --demo
+```
+
+For Vicon:
+
+```sh
+pip install './pose[vicon]'
+pose.vicon --host 192.154.4.124 --list  # List current object names
+pose.vicon --host 192.154.4.124 --subject crazyflie | pose
+```
+
+Vicon uses the root segment; override with `--segment NAME`. Server defaults:
+`127.0.0.1:8765`; change with `--host`/`--port`. Stop with Ctrl-C.
+
+## Pose API
+
+Connect to `ws://127.0.0.1:8765/pose`. Each text message contains:
+
+```json
+{"timestamp":1789600000.125,"position":[0.1,-0.2,0.5],"quaternion":[0,0,0,1]}
+```
+
+- `timestamp`: Unix seconds when the producer received the sample.
+- `position`: meters, right-handed world frame: X forward, Y left, Z up.
+- `quaternion`: normalized `[x,y,z,w]`, rotating body-local vectors into world coordinates.
+
+All values must be finite numbers. The Vicon adapter converts millimeters to meters
+and reorders its SDK quaternion to this shared convention.
+
+The stream is read-only. Invalid/occluded poses are skipped; tracking loss means
+silence. Clients should detect stale data. Slow clients skip pending samples;
+new connections receive only new samples. Input EOF stops the server.
+
+Other adapters must emit this format as flushed, newline-delimited JSON to
+stdout, with diagnostics on stderr:
+
+```sh
+python -u my_mocap_adapter.py | pose
+```
+
+## Browser
+
+Run Python on the browser's computer. From a Connect button handler:
+
+```js
+const response = await fetch("http://127.0.0.1:8765/health");
+if (!response.ok) throw new Error("Pose server unavailable");
+const socket = new WebSocket("ws://127.0.0.1:8765/pose");
+socket.onmessage = ({data}) => console.log(JSON.parse(data));
+```
+
+Grant Chrome's local/loopback permission. Allowed origins: `https://rc.rl.tools`,
+`http://localhost:8000`, `http://127.0.0.1:8000`; extend with `--allow-origin URL`.
+`/health` returns `last_pose_age_s` (`null` before any sample).
+
+Tests: `python -B -m unittest discover -s pose/tests -v`.

poseproxy-0.0.1/build.sh

@@ -0,0 +1,5 @@
+# auth in .pypirc
+rm -rf ../pose/dist
+pip install --upgrade build twine
+python3 -m build --sdist
+python3 -m twine upload dist/*

poseproxy-0.0.1/pose_server.py

@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+"""Forward newline-delimited JSON poses from stdin to ws://127.0.0.1:8765/pose."""
+
+import argparse
+import asyncio
+import concurrent.futures
+import json
+import logging
+import math
+import signal
+import sys
+import threading
+import time
+
+from websockets.asyncio.server import serve
+from websockets.exceptions import ConnectionClosed
+
+
+LOG = logging.getLogger("pose_server")
+DEFAULT_ORIGINS = (
+    "https://rc.rl.tools",
+    "http://localhost:8000",
+    "http://127.0.0.1:8000",
+)
+
+
+def encode_pose(pose):
+    """Validate the common pose format and normalize its xyzw quaternion."""
+    def number(value):
+        if isinstance(value, bool) or not isinstance(value, (int, float)):
+            raise ValueError("pose values must be numbers")
+        value = float(value)
+        if not math.isfinite(value):
+            raise ValueError("pose values must be finite")
+        return value
+
+    def vector(name, size):
+        value = pose.get(name)
+        if not isinstance(value, list) or len(value) != size:
+            raise ValueError(f"{name} must be an array of {size} numbers")
+        return [number(item) for item in value]
+
+    if not isinstance(pose, dict):
+        raise ValueError("pose must be a JSON object")
+    timestamp = number(pose.get("timestamp"))
+    position = vector("position", 3)
+    quaternion = vector("quaternion", 4)
+    norm = math.hypot(*quaternion)
+    if norm == 0 or not math.isfinite(norm):
+        raise ValueError("quaternion must have a finite, nonzero norm")
+    return json.dumps({
+        "timestamp": timestamp,
+        "position": position,
+        "quaternion": [value / norm for value in quaternion],
+    }, separators=(",", ":"), allow_nan=False)
+
+
+class PoseServer:
+    def __init__(self, origins=DEFAULT_ORIGINS):
+        self.origins = set(origins)
+        self.clients = set()
+        self.last_received = None
+
+    async def publish(self, message):
+        """Publish an encoded pose on the event loop; retain one pending pose/client."""
+        self.last_received = time.monotonic()
+        for queue in self.clients:
+            if queue.full():
+                queue.get_nowait()
+            queue.put_nowait(message)
+
+    def process_request(self, connection, request):
+        origins = request.headers.get_all("Origin")
+        if len(origins) > 1 or (origins and origins[0] not in self.origins):
+            return connection.respond(403, "Origin not allowed\n")
+        if request.path == "/health":
+            age = (None if self.last_received is None else
+                   time.monotonic() - self.last_received)
+            response = connection.respond(200, json.dumps({
+                "last_pose_age_s": age,
+            }) + "\n")
+            del response.headers["Content-Type"]
+            response.headers["Content-Type"] = "application/json"
+            response.headers["Cache-Control"] = "no-store"
+            response.headers["Vary"] = "Origin"
+            if origins:
+                response.headers["Access-Control-Allow-Origin"] = origins[0]
+            return response
+        if request.path != "/pose":
+            return connection.respond(404, "Use /pose or /health\n")
+        return None
+
+    async def handle(self, websocket):
+        queue = asyncio.Queue(maxsize=1)
+        self.clients.add(queue)
+
+        async def send_poses():
+            while True:
+                message = await queue.get()
+                try:
+                    await asyncio.wait_for(websocket.send(message), timeout=1)
+                except asyncio.TimeoutError:
+                    await websocket.close(code=1013, reason="Client is too slow")
+                    return
+
+        async def receive():
+            # Reading also detects disconnects when the mocap source is silent.
+            async for _ in websocket:
+                await websocket.close(code=1008, reason="Pose stream is read-only")
+                return
+
+        tasks = [asyncio.create_task(send_poses()), asyncio.create_task(receive())]
+        try:
+            done, _ = await asyncio.wait(tasks, return_when=asyncio.FIRST_COMPLETED)
+            for task in done:
+                task.result()
+        except ConnectionClosed:
+            pass
+        finally:
+            self.clients.discard(queue)
+            for task in tasks:
+                task.cancel()
+            await asyncio.gather(*tasks, return_exceptions=True)
+
+    def listen(self, host="127.0.0.1", port=8765):
+        return serve(
+            self.handle, host, port, process_request=self.process_request,
+            origins=[None, *self.origins], compression=None,
+            max_size=1024, max_queue=1, write_limit=4096, close_timeout=1,
+        )
+
+
+def read_stdin(fd, loop, server, finished):
+    """A daemon reader keeps stdin blocking and SDK work out of the event loop."""
+    try:
+        # Unbuffered IO avoids a blocked buffered-reader lock during Ctrl-C exit.
+        with open(fd, "rb", buffering=0, closefd=False) as stream:
+            while line := stream.readline(4097):
+                if len(line) > 4096:
+                    LOG.warning("Skipping input line longer than 4096 bytes")
+                    while line and not line.endswith(b"\n"):
+                        line = stream.readline(4097)
+                    continue
+                if not line.strip():
+                    continue
+                try:
+                    message = encode_pose(json.loads(line))
+                except (ValueError, OverflowError, RecursionError) as error:
+                    LOG.warning("Skipping invalid pose: %s", error)
+                    continue
+                # At most one callback is outstanding, even for a fast producer.
+                publication = server.publish(message)
+                try:
+                    future = asyncio.run_coroutine_threadsafe(publication, loop)
+                except RuntimeError:
+                    publication.close()
+                    return
+                future.result()
+    except (RuntimeError, concurrent.futures.CancelledError):
+        pass  # Event loop shut down while stdin was still open.
+    except OSError as error:
+        LOG.error("Reading poses failed: %s", error)
+    finally:
+        try:
+            loop.call_soon_threadsafe(finished.set)
+        except RuntimeError:
+            pass
+
+
+async def demo(server, rate):
+    start = time.monotonic()
+    while True:
+        angle = (time.monotonic() - start) * 0.5
+        await server.publish(encode_pose({
+            "timestamp": time.time(),
+            "position": [0.5 * math.cos(angle), 0.5 * math.sin(angle), 1.0],
+            "quaternion": [0.0, 0.0, math.sin(angle / 2), math.cos(angle / 2)],
+        }))
+        await asyncio.sleep(1 / rate)
+
+
+async def run(args):
+    loop = asyncio.get_running_loop()
+    finished = asyncio.Event()
+    # asyncio.run handles Ctrl-C; also shut down cleanly on POSIX SIGTERM.
+    try:
+        loop.add_signal_handler(signal.SIGTERM, finished.set)
+    except NotImplementedError:
+        pass
+    server = PoseServer((*DEFAULT_ORIGINS, *args.allow_origin))
+    async with server.listen(args.host, args.port) as listener:
+        port = listener.sockets[0].getsockname()[1]
+        LOG.info("Pose stream: ws://%s:%s/pose", args.host, port)
+        if args.demo:
+            producer = asyncio.create_task(demo(server, args.rate))
+        else:
+            producer = None
+            threading.Thread(
+                target=read_stdin,
+                args=(sys.stdin.fileno(), loop, server, finished), daemon=True,
+            ).start()
+        try:
+            await finished.wait()
+        finally:
+            if producer is not None:
+                producer.cancel()
+                await asyncio.gather(producer, return_exceptions=True)
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--host", default="127.0.0.1", help="Bind address")
+    parser.add_argument("--port", type=int, default=8765)
+    parser.add_argument("--allow-origin", action="append", default=[],
+                        help="Additional browser origin (repeatable)")
+    parser.add_argument("--demo", action="store_true", help="Generate poses instead of reading stdin")
+    parser.add_argument("--rate", type=float, default=100, help="Demo rate in Hz (default: 100)")
+    args = parser.parse_args()
+    if not 0 <= args.port <= 65535:
+        parser.error("port must be between 0 and 65535")
+    if not math.isfinite(args.rate) or args.rate <= 0:
+        parser.error("rate must be finite and positive")
+    logging.basicConfig(level=logging.INFO, format="%(name)s: %(message)s")
+    try:
+        asyncio.run(run(args))
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    main()

poseproxy-0.0.1/pyproject.toml

@@ -0,0 +1,23 @@
+[build-system]
+requires = ["hatchling"]
+build-backend = "hatchling.build"
+
+[project]
+name = "poseproxy"
+version = "0.0.1"
+description = "Simple Pose Forwarder For Motion Capturing Systems"
+readme = "README.md"
+authors = [{name = "Jonas Eschmann", email = "jonas.eschmann@gmail.com"}]
+license = {text = "MIT"}
+requires-python = ">=3.10"
+dependencies = ["websockets>=15.0.1,<16"]
+
+[project.optional-dependencies]
+vicon = ["pyvicon-datastream==0.2.4"]
+
+[tool.hatch.build.targets.wheel]
+only-include = ["pose_server.py", "vicon_poses.py"]
+
+[project.scripts]
+pose = "pose_server:main"
+"pose.vicon" = "vicon_poses:main"

poseproxy-0.0.1/requirements.txt

@@ -0,0 +1 @@
+websockets>=15.0.1,<16

poseproxy-0.0.1/tests/test_pose_server.py

@@ -0,0 +1,272 @@
+import asyncio
+from contextlib import redirect_stderr, redirect_stdout
+from io import StringIO
+import json
+from pathlib import Path
+import re
+import signal
+import sys
+import unittest
+from unittest.mock import Mock, patch
+
+from websockets.asyncio.client import connect
+from websockets.exceptions import ConnectionClosed, InvalidStatus
+
+POSE_DIR = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(POSE_DIR))
+from pose_server import PoseServer, encode_pose
+from vicon_poses import main as vicon_main, poses
+
+
+POSE = {"timestamp": 1789600000.125, "position": [1, 2, 3], "quaternion": [0, 0, 0, 1]}
+ORIGIN = "https://rc.rl.tools"
+
+
+class FormatTests(unittest.TestCase):
+    def test_normalizes_quaternion_and_preserves_pose(self):
+        result = json.loads(encode_pose({**POSE, "quaternion": [0, 0, 2, 2]}))
+        self.assertEqual(result["position"], POSE["position"])
+        self.assertEqual(result["timestamp"], POSE["timestamp"])
+        self.assertAlmostEqual(result["quaternion"][2], 2 ** -0.5)
+        self.assertAlmostEqual(result["quaternion"][3], 2 ** -0.5)
+
+    def test_rejects_invalid_measurements(self):
+        invalid = [
+            None, [], {}, {**POSE, "position": [1, 2]},
+            {**POSE, "position": [1, float("nan"), 3]},
+            {**POSE, "quaternion": [0, 0, 0, 0]},
+            {**POSE, "quaternion": [0, 0, 0, float("inf")]},
+            {**POSE, "timestamp": True}, {**POSE, "timestamp": "123"},
+        ]
+        for value in invalid:
+            with self.subTest(value=value), self.assertRaises(ValueError):
+                encode_pose(value)
+
+
+class ViconTests(unittest.TestCase):
+    def make_sdk(self):
+        sdk = Mock()
+        client = sdk.PyViconDatastream.return_value
+        for method in ("connect", "enable_segment_data", "set_stream_mode", "set_axis_mapping", "get_frame"):
+            getattr(client, method).return_value = sdk.Result.Success
+        client.get_subject_root_segment_name.return_value = "root"
+        client.get_segment_global_translation.side_effect = [None, [1000, -2000, 500]]
+        client.get_segment_global_quaternion.return_value = [0.5, 0.5, -0.5, 0.5]
+        return sdk, client
+
+    def test_occlusion_units_order_and_cleanup(self):
+        sdk, client = self.make_sdk()
+        stream = poses(sdk, "vicon:801", "drone")
+        pose = next(stream)
+        self.assertEqual(pose["position"], [1, -2, 0.5])
+        self.assertEqual(pose["quaternion"], [0.5, -0.5, 0.5, 0.5])
+        self.assertGreater(pose["timestamp"], 0)
+        self.assertEqual(client.get_frame.call_count, 2)
+        client.set_buffer_size.assert_called_once_with(1)
+        client.set_axis_mapping.assert_called_once_with(
+            sdk.Direction.Forward, sdk.Direction.Left, sdk.Direction.Up)
+        client.get_segment_global_translation.assert_called_with("drone", "root")
+        stream.close()
+        client.disconnect.assert_called_once()
+
+    def test_source_failure_is_reported_and_disconnected(self):
+        sdk, client = self.make_sdk()
+        client.get_frame.return_value = sdk.Result.NotConnected
+        with self.assertRaisesRegex(RuntimeError, "get frame failed"):
+            next(poses(sdk, "vicon", "drone"))
+        client.disconnect.assert_called_once()
+
+    def run_list_command(self, sdk):
+        stdout, stderr = StringIO(), StringIO()
+        with patch.dict(sys.modules, {"pyvicon_datastream": sdk}), \
+                patch.object(sys, "argv", ["pose.vicon", "--host", "vicon", "--list"]), \
+                redirect_stdout(stdout), redirect_stderr(stderr):
+            vicon_main()
+        return stdout.getvalue(), stderr.getvalue()
+
+    def test_list_waits_for_frame_and_prints_names_without_streaming(self):
+        sdk, client = self.make_sdk()
+        client.get_frame.side_effect = [sdk.Result.NoFrame, sdk.Result.Success]
+        client.get_subject_count.return_value = 2
+        client.get_subject_name.side_effect = ["crazyflie", "Calibration Wand"]
+        stdout, stderr = self.run_list_command(sdk)
+        self.assertEqual(stdout, "crazyflie\nCalibration Wand\n")
+        self.assertEqual(stderr, "")
+        self.assertEqual(client.get_frame.call_count, 2)
+        client.get_subject_root_segment_name.assert_not_called()
+        client.get_segment_global_translation.assert_not_called()
+        client.disconnect.assert_called_once()
+
+    def test_list_handles_empty_frame(self):
+        sdk, client = self.make_sdk()
+        client.get_subject_count.return_value = 0
+        stdout, stderr = self.run_list_command(sdk)
+        self.assertEqual(stdout, "")
+        self.assertEqual(stderr, "No objects found.\n")
+        client.disconnect.assert_called_once()
+
+    def test_list_connection_failure_exits_with_error_and_disconnects(self):
+        sdk, client = self.make_sdk()
+        client.connect.return_value = sdk.Result.ClientConnectionFailed
+        with self.assertRaises(SystemExit) as caught:
+            self.run_list_command(sdk)
+        self.assertEqual(caught.exception.code, 1)
+        client.get_subject_count.assert_not_called()
+        client.disconnect.assert_called_once()
+
+
+async def http_get(port, path="/health", origin=ORIGIN):
+    reader, writer = await asyncio.open_connection("127.0.0.1", port)
+    origin_header = "" if origin is None else f"Origin: {origin}\r\n"
+    writer.write((f"GET {path} HTTP/1.1\r\nHost: 127.0.0.1:{port}\r\n"
+                  f"{origin_header}Connection: close\r\n\r\n").encode())
+    await writer.drain()
+    response = await asyncio.wait_for(reader.read(), 2)
+    writer.close()
+    await writer.wait_closed()
+    header, body = response.decode().split("\r\n\r\n", 1)
+    return header, body
+
+
+class ServerTests(unittest.IsolatedAsyncioTestCase):
+    async def asyncSetUp(self):
+        self.server = PoseServer()
+        self.listener = await self.server.listen(port=0)
+        self.port = self.listener.sockets[0].getsockname()[1]
+        self.url = f"ws://127.0.0.1:{self.port}/pose"
+
+    async def asyncTearDown(self):
+        self.listener.close()
+        await self.listener.wait_closed()
+
+    def connect(self, **kwargs):
+        return connect(self.url, origin=ORIGIN, proxy=None, **kwargs)
+
+    async def test_fanout_without_replaying_stale_poses(self):
+        await self.server.publish(encode_pose(POSE))
+        async with self.connect() as first, self.connect() as second:
+            with self.assertRaises(asyncio.TimeoutError):
+                await asyncio.wait_for(first.recv(), 0.05)
+            next_pose = {**POSE, "timestamp": POSE["timestamp"] + 1}
+            await self.server.publish(encode_pose(next_pose))
+            for client in (first, second):
+                received = json.loads(await asyncio.wait_for(client.recv(), 1))
+                self.assertEqual(received, next_pose)
+
+    async def test_health_cors_and_origin_rejection(self):
+        headers, body = await http_get(self.port)
+        self.assertIn("200 OK", headers)
+        self.assertIn(f"Access-Control-Allow-Origin: {ORIGIN}", headers)
+        self.assertEqual(headers.lower().count("content-type:"), 1)
+        self.assertEqual(json.loads(body), {"last_pose_age_s": None})
+        await self.server.publish(encode_pose(POSE))
+        _, body = await http_get(self.port)
+        self.assertGreaterEqual(json.loads(body)["last_pose_age_s"], 0)
+        headers, _ = await http_get(self.port, origin="https://unapproved.example")
+        self.assertIn("403 Forbidden", headers)
+        self.assertNotIn("Access-Control-Allow-Origin", headers)
+        with self.assertRaises(InvalidStatus) as caught:
+            async with connect(self.url, origin="https://unapproved.example", proxy=None):
+                pass
+        self.assertEqual(caught.exception.response.status_code, 403)
+        headers, _ = await http_get(self.port, path="/unknown")
+        self.assertIn("404 Not Found", headers)
+
+    async def test_client_cannot_inject_poses(self):
+        async with self.connect() as client:
+            await client.send(json.dumps(POSE))
+            with self.assertRaises(ConnectionClosed):
+                await asyncio.wait_for(client.recv(), 1)
+            self.assertEqual(client.close_code, 1008)
+
+    async def test_slow_client_skips_pending_frames_without_blocking_others(self):
+        blocked = asyncio.Event()
+        release = asyncio.Event()
+        received = asyncio.Queue()
+
+        class SlowClient:
+            async def send(self, message):
+                blocked.set()
+                await release.wait()
+                await received.put(json.loads(message))
+
+            def __aiter__(self):
+                return self
+
+            async def __anext__(self):
+                await asyncio.Future()
+
+        task = asyncio.create_task(self.server.handle(SlowClient()))
+        try:
+            async with self.connect() as fast:
+                await self.server.publish(encode_pose(POSE))
+                await asyncio.wait_for(blocked.wait(), 1)
+                await asyncio.wait_for(fast.recv(), 1)
+                for timestamp in (2, 3, 4):
+                    await self.server.publish(encode_pose({**POSE, "timestamp": timestamp}))
+                self.assertEqual(json.loads(await asyncio.wait_for(fast.recv(), 1))["timestamp"], 4)
+                release.set()
+                self.assertEqual((await asyncio.wait_for(received.get(), 1))["timestamp"], POSE["timestamp"])
+                self.assertEqual((await asyncio.wait_for(received.get(), 1))["timestamp"], 4)
+        finally:
+            task.cancel()
+            await asyncio.gather(task, return_exceptions=True)
+
+
+class CommandTests(unittest.IsolatedAsyncioTestCase):
+    async def launch(self, *args):
+        process = await asyncio.create_subprocess_exec(
+            sys.executable, "-B", str(POSE_DIR / "pose_server.py"), "--port", "0", *args,
+            stdin=asyncio.subprocess.PIPE, stdout=asyncio.subprocess.PIPE,
+            stderr=asyncio.subprocess.PIPE,
+        )
+        async def cleanup():
+            if process.returncode is None:
+                process.terminate()
+            await asyncio.wait_for(process.communicate(), 5)
+        self.addAsyncCleanup(cleanup)
+        while line := await asyncio.wait_for(process.stderr.readline(), 5):
+            if match := re.search(rb"Pose stream: ws://127.0.0.1:(\d+)/pose", line):
+                return process, int(match[1])
+        self.fail("server did not start")
+
+    async def test_stdin_validation_streaming_and_eof(self):
+        process, port = await self.launch()
+        async with connect(f"ws://127.0.0.1:{port}/pose", origin=ORIGIN, proxy=None) as client:
+            process.stdin.write(b"not json\n" + b"x" * 5000 + b"\n" + json.dumps(POSE).encode() + b"\n")
+            await process.stdin.drain()
+            self.assertEqual(json.loads(await asyncio.wait_for(client.recv(), 2)), POSE)
+            process.stdin.close()
+            await asyncio.wait_for(process.wait(), 3)
+            self.assertEqual(process.returncode, 0)
+            with self.assertRaises(ConnectionClosed):
+                await asyncio.wait_for(client.recv(), 1)
+
+    async def test_demo_and_shutdown_with_open_stdin(self):
+        process, port = await self.launch("--demo", "--rate", "50")
+        async with connect(f"ws://127.0.0.1:{port}/pose", origin=ORIGIN, proxy=None) as client:
+            first = json.loads(await asyncio.wait_for(client.recv(), 2))
+            second = json.loads(await asyncio.wait_for(client.recv(), 2))
+            self.assertGreater(second["timestamp"], first["timestamp"])
+            self.assertEqual(first["position"][2], 1)
+            self.assertAlmostEqual(sum(q*q for q in first["quaternion"]), 1)
+        process.terminate()
+        await asyncio.wait_for(process.wait(), 3)
+        self.assertEqual(process.returncode, 0)
+
+    async def test_shutdown_while_waiting_for_stdin(self):
+        process, _ = await self.launch()
+        process.terminate()
+        await asyncio.wait_for(process.wait(), 3)
+        self.assertEqual(process.returncode, 0)
+
+    @unittest.skipIf(sys.platform == "win32", "POSIX Ctrl-C signal")
+    async def test_ctrl_c_while_waiting_for_stdin(self):
+        process, _ = await self.launch()
+        process.send_signal(signal.SIGINT)
+        await asyncio.wait_for(process.wait(), 3)
+        self.assertEqual(process.returncode, 0)
+
+
+if __name__ == "__main__":
+    unittest.main()

poseproxy-0.0.1/vicon_poses.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+"""Emit one Vicon body's poses as newline-delimited JSON for pose_server.py."""
+
+import argparse
+from contextlib import closing
+import json
+import sys
+import time
+
+
+def frames(sdk, host):
+    client = sdk.PyViconDatastream()
+
+    def check(result, action):
+        if result != sdk.Result.Success:
+            raise RuntimeError(f"Vicon {action} failed: {result}")
+
+    try:
+        check(client.connect(host), f"connection to {host}")
+        check(client.enable_segment_data(), "enable segment data")
+        client.set_buffer_size(1)
+        check(client.set_stream_mode(sdk.StreamMode.ServerPush), "set stream mode")
+        check(client.set_axis_mapping(
+            sdk.Direction.Forward, sdk.Direction.Left, sdk.Direction.Up,
+        ), "set axis mapping")
+        while True:
+            result = client.get_frame()
+            if result == sdk.Result.NoFrame:
+                time.sleep(0.001)
+                continue
+            check(result, "get frame")
+            yield client, time.time()
+    finally:
+        client.disconnect()
+
+
+def list_objects(sdk, host):
+    with closing(frames(sdk, host)) as stream:
+        client, _ = next(stream)
+        return [client.get_subject_name(index)
+                for index in range(client.get_subject_count())]
+
+
+def poses(sdk, host, subject, segment=None):
+    with closing(frames(sdk, host)) as stream:
+        for client, timestamp in stream:
+            if segment is None:
+                segment = client.get_subject_root_segment_name(subject)
+                if not segment:
+                    raise RuntimeError(f"Vicon subject {subject!r} has no root segment")
+            position = client.get_segment_global_translation(subject, segment)
+            rotation = client.get_segment_global_quaternion(subject, segment)
+            # The wrapper returns None for occluded or unavailable segment data.
+            if position is None or rotation is None:
+                continue
+            w, x, y, z = map(float, rotation)
+            yield {
+                "timestamp": timestamp,
+                "position": [float(value) * 0.001 for value in position],
+                "quaternion": [x, y, z, w],
+            }
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--host", required=True, help="Vicon host[:port] (default port: 801)")
+    parser.add_argument("--subject", default="crazyflie")
+    parser.add_argument("--segment", help="Default: subject's root segment")
+    parser.add_argument("--list", action="store_true", dest="list_objects",
+                        help="List current object (subject) names and exit")
+    args = parser.parse_args()
+    try:
+        import pyvicon_datastream as sdk
+    except ImportError:
+        parser.exit(1, "Install the Vicon adapter dependency: pip install pyvicon-datastream==0.2.4\n")
+    stream = None
+    try:
+        if args.list_objects:
+            names = list_objects(sdk, args.host)
+            for name in names:
+                print(name)
+            if not names:
+                print("No objects found.", file=sys.stderr)
+            return
+        stream = poses(sdk, args.host, args.subject, args.segment)
+        for pose in stream:
+            print(json.dumps(pose, allow_nan=False), flush=True)
+    except (KeyboardInterrupt, BrokenPipeError):
+        pass
+    except (RuntimeError, ValueError) as error:
+        parser.exit(1, f"{error}\n")
+    finally:
+        if stream is not None:
+            stream.close()
+        # Avoid retrying a failed stdout flush during interpreter shutdown.
+        if sys.stdout is not None:
+            try:
+                sys.stdout.flush()
+            except BrokenPipeError:
+                sys.stdout = None
+
+
+if __name__ == "__main__":
+    main()