Add D455 sensor host and shared Node/Core preparation surface
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,13 @@
|
||||
"""Isolated interpreter; only root-owned pinned runtime and product code on sys.path."""
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
root = Path("/var/lib/mission-core-node-drivers")
|
||||
path = Path((root / "active.path").read_text())
|
||||
if path.parent != root or not path.name.isalnum() or path.is_symlink() or path.stat().st_uid != 0:
|
||||
raise RuntimeError("Invalid driver installation")
|
||||
sys.path[:0] = [str(path), "/usr/lib/mission-core-node/sensors", "/usr/lib/mission-core-node/sdk"]
|
||||
from server import main # noqa: E402 — use only the verified isolated runtime above
|
||||
|
||||
main()
|
||||
@@ -0,0 +1,503 @@
|
||||
"""D455 adapter. Hardware ownership and raw acquisition remain on the board."""
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import queue
|
||||
import shutil
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from contextlib import suppress
|
||||
from datetime import UTC, datetime
|
||||
|
||||
import numpy as np
|
||||
import pyrealsense2 as rs
|
||||
from missioncore_plugin_sdk.v0alpha2.session import DeviceSessionSnapshot
|
||||
|
||||
MODEL = {
|
||||
"plugin_id": "missioncore.realsense",
|
||||
"plugin_version": "0.6.0",
|
||||
"model_id": "realsense.d455",
|
||||
}
|
||||
|
||||
|
||||
def utc():
|
||||
return datetime.now(UTC).isoformat()
|
||||
|
||||
|
||||
def device_id(serial):
|
||||
return "rsd455_" + hashlib.sha256(serial.encode()).hexdigest()[:32]
|
||||
|
||||
|
||||
def atomic(path, value):
|
||||
tmp = path.with_suffix(".tmp")
|
||||
with tmp.open("w") as f:
|
||||
json.dump(value, f, ensure_ascii=False, allow_nan=False)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
tmp.replace(path)
|
||||
|
||||
|
||||
def kind(profile):
|
||||
return str(profile.stream_type()).split(".")[-1]
|
||||
|
||||
|
||||
class Device:
|
||||
def __init__(self, serial, root, execution):
|
||||
self.serial = serial
|
||||
self.id = device_id(serial)
|
||||
self.root = root / self.id
|
||||
self.root.mkdir(exist_ok=True, mode=0o700)
|
||||
self.lock = threading.RLock()
|
||||
self.execution = execution
|
||||
self.session = "sensor_" + uuid.uuid4().hex
|
||||
self.opened = utc()
|
||||
self.revision = 0
|
||||
self.online = True
|
||||
self.acquisition = "idle"
|
||||
self.message = ""
|
||||
self.pipeline = None
|
||||
self.thread = None
|
||||
self.queue = queue.Queue(maxsize=2)
|
||||
self.stop_event = threading.Event()
|
||||
self.images = {}
|
||||
self.motion = {}
|
||||
self.depth = None
|
||||
self.intrinsics = None
|
||||
self.frames = {}
|
||||
self.last_frame = None
|
||||
self.record = None
|
||||
self.profiles = []
|
||||
self.options = []
|
||||
self.sdk_device = None
|
||||
self.firmware = ""
|
||||
self.transport = ""
|
||||
self.config = {"name": "RealSense D455", "verified": False}
|
||||
if (self.root / "config.json").exists():
|
||||
self.config.update(json.loads((self.root / "config.json").read_text()))
|
||||
# A previous verification is evidence, not a live readiness assertion.
|
||||
self.verified_this_process = False
|
||||
for path in self.root.glob("recordings/*/manifest.json"):
|
||||
value = json.loads(path.read_text())
|
||||
if value.get("state") == "recording":
|
||||
value.update(state="interrupted", recovered_at=utc())
|
||||
atomic(path, value)
|
||||
|
||||
def save(self):
|
||||
atomic(self.root / "config.json", self.config)
|
||||
|
||||
def refresh(self, dev):
|
||||
with self.lock:
|
||||
self.sdk_device = dev
|
||||
self.online = True
|
||||
self.firmware = dev.get_info(rs.camera_info.firmware_version)
|
||||
self.transport = dev.get_info(rs.camera_info.usb_type_descriptor)
|
||||
if self.pipeline is not None:
|
||||
return
|
||||
profiles, options = [], []
|
||||
for index, sensor in enumerate(dev.query_sensors()):
|
||||
for profile in sensor.get_stream_profiles():
|
||||
stream = kind(profile)
|
||||
if stream not in ("color", "depth", "infrared", "accel", "gyro"):
|
||||
continue
|
||||
data = {
|
||||
"sensor": index,
|
||||
"stream": stream,
|
||||
"index": profile.stream_index(),
|
||||
"fps": profile.fps(),
|
||||
"format": str(profile.format()).split(".")[-1],
|
||||
}
|
||||
if profile.is_video_stream_profile():
|
||||
video = profile.as_video_stream_profile()
|
||||
data.update(width=video.width(), height=video.height())
|
||||
data["id"] = hashlib.sha256(
|
||||
json.dumps(data, sort_keys=True).encode()
|
||||
).hexdigest()[:16]
|
||||
profiles.append(data)
|
||||
for option in sensor.get_supported_options():
|
||||
try:
|
||||
limits = sensor.get_option_range(option)
|
||||
value = sensor.get_option(option)
|
||||
if not all(
|
||||
math.isfinite(x) for x in (limits.min, limits.max, limits.step, value)
|
||||
):
|
||||
continue
|
||||
options.append(
|
||||
{
|
||||
"id": f"{index}:{int(option)}",
|
||||
"sensor": sensor.get_info(rs.camera_info.name),
|
||||
"label": str(option).split(".")[-1],
|
||||
"value": value,
|
||||
"min": limits.min,
|
||||
"max": limits.max,
|
||||
"step": limits.step,
|
||||
"read_only": sensor.is_option_read_only(option),
|
||||
}
|
||||
)
|
||||
except RuntimeError:
|
||||
continue
|
||||
self.profiles, self.options = profiles, options
|
||||
|
||||
def defaults(self):
|
||||
result = []
|
||||
for stream, index, fmt, fps in [
|
||||
("depth", 0, "z16", 15),
|
||||
("color", 0, "rgb8", 15),
|
||||
("infrared", 1, "y8", 15),
|
||||
("infrared", 2, "y8", 15),
|
||||
("accel", 0, "motion_xyz32f", 63),
|
||||
("gyro", 0, "motion_xyz32f", 200),
|
||||
]:
|
||||
candidates = [
|
||||
p
|
||||
for p in self.profiles
|
||||
if p["stream"] == stream and p["index"] == index and p["format"] == fmt
|
||||
]
|
||||
if candidates:
|
||||
p = min(
|
||||
candidates,
|
||||
key=lambda p: (
|
||||
abs(p.get("width", 640) - 640)
|
||||
+ abs(p.get("height", 480) - 480)
|
||||
+ abs(p["fps"] - fps) * 20
|
||||
),
|
||||
)
|
||||
result.append(p["id"])
|
||||
return result
|
||||
|
||||
def snapshot(self, detailed=True):
|
||||
with self.lock:
|
||||
now = utc()
|
||||
snap = DeviceSessionSnapshot.model_validate(
|
||||
{
|
||||
"context": {
|
||||
"session_id": self.session,
|
||||
"device": {
|
||||
"device_id": self.id,
|
||||
"model": MODEL,
|
||||
"stability": "stable",
|
||||
"basis": "hardware-identifier",
|
||||
},
|
||||
"execution": self.execution,
|
||||
"opened_at": self.opened,
|
||||
},
|
||||
"revision": self.revision,
|
||||
"enrollment": "enrolled" if self.config["verified"] else "empty",
|
||||
"connectivity": "connected" if self.online else "offline",
|
||||
"acquisition": self.acquisition,
|
||||
"observed_at": now,
|
||||
"message": self.message or None,
|
||||
}
|
||||
)
|
||||
value = {
|
||||
"id": self.id,
|
||||
"name": self.config["name"],
|
||||
"model": "RealSense D455",
|
||||
"prepared": True,
|
||||
"verified": self.verified_this_process,
|
||||
"online": self.online,
|
||||
"snapshot": snap.model_dump(mode="json"),
|
||||
"firmware": self.firmware,
|
||||
"usb": self.transport,
|
||||
"frames": dict(self.frames),
|
||||
"last_frame": self.last_frame,
|
||||
"recording": self.record,
|
||||
"layers": list(self.images)
|
||||
+ (["points"] if self.depth is not None else [])
|
||||
+ (["motion"] if self.motion else []),
|
||||
}
|
||||
if detailed:
|
||||
value.update(
|
||||
profiles=self.profiles,
|
||||
defaults=self.defaults(),
|
||||
options=self.options,
|
||||
recordings=self.recordings(),
|
||||
motion=dict(self.motion),
|
||||
)
|
||||
return value
|
||||
|
||||
def callback(self, frame):
|
||||
if frame.is_motion_frame():
|
||||
v = frame.as_motion_frame().get_motion_data()
|
||||
key = kind(frame.profile)
|
||||
self.motion[key] = {
|
||||
"x": v.x,
|
||||
"y": v.y,
|
||||
"z": v.z,
|
||||
"timestamp_ms": frame.get_timestamp(),
|
||||
"clock": str(frame.get_frame_timestamp_domain()),
|
||||
"observed_at": utc(),
|
||||
}
|
||||
self.frames[key] = self.frames.get(key, 0) + 1
|
||||
elif frame.is_frameset():
|
||||
with suppress(queue.Full):
|
||||
self.queue.put_nowait(frame.as_frameset())
|
||||
|
||||
def start(self, selected=None, record=False):
|
||||
with self.lock:
|
||||
if self.pipeline is not None:
|
||||
raise ValueError("Захват уже запущен. Сначала остановите его.")
|
||||
if not self.online or self.sdk_device is None:
|
||||
raise ValueError("Камера не подключена.")
|
||||
ids = selected if selected is not None else self.defaults()
|
||||
if not isinstance(ids, list) or not 1 <= len(ids) <= 6 or len(set(ids)) != len(ids):
|
||||
raise ValueError("Выберите профили потоков.")
|
||||
profiles = [next((p for p in self.profiles if p["id"] == ident), None) for ident in ids]
|
||||
if any(p is None for p in profiles) or len(
|
||||
{(p["stream"], p["index"]) for p in profiles}
|
||||
) != len(profiles):
|
||||
raise ValueError("Выбраны несовместимые профили потоков.")
|
||||
if not any(p["stream"] in ("depth", "color", "infrared") for p in profiles):
|
||||
raise ValueError("Выберите хотя бы один видеопоток.")
|
||||
config = rs.config()
|
||||
config.enable_device(self.serial)
|
||||
for p in profiles:
|
||||
stream, fmt = getattr(rs.stream, p["stream"]), getattr(rs.format, p["format"])
|
||||
if "width" in p:
|
||||
config.enable_stream(stream, p["index"], p["width"], p["height"], fmt, p["fps"])
|
||||
else:
|
||||
config.enable_stream(stream, p["index"], fmt, p["fps"])
|
||||
pipeline = rs.pipeline()
|
||||
if not config.can_resolve(rs.pipeline_wrapper(pipeline)):
|
||||
raise ValueError("Камера не поддерживает эту комбинацию профилей. Выберите другую.")
|
||||
self.acquisition = "starting"
|
||||
self.revision += 1
|
||||
self.images, self.motion, self.frames = {}, {}, {}
|
||||
self.depth, self.last_frame = None, None
|
||||
self.queue = queue.Queue(maxsize=2)
|
||||
self.stop_event.clear()
|
||||
record_path = None
|
||||
if record:
|
||||
if shutil.disk_usage(self.root).free < 2 * 1024**3:
|
||||
self.acquisition = "idle"
|
||||
raise ValueError("Для исходной записи нужно не меньше 2 ГиБ свободного места.")
|
||||
ident = "capture_" + uuid.uuid4().hex
|
||||
record_path = self.root / "recordings" / ident
|
||||
record_path.mkdir(parents=True, mode=0o700)
|
||||
config.enable_record_to_file(str(record_path / "source.bag"))
|
||||
self.record = {
|
||||
"id": ident,
|
||||
"state": "recording",
|
||||
"started_at": utc(),
|
||||
"monotonic_ns": time.monotonic_ns(),
|
||||
"device_id": self.id,
|
||||
"session_id": self.session,
|
||||
"profiles": profiles,
|
||||
"firmware": self.firmware,
|
||||
"sdk": "2.58.4.10922",
|
||||
"options": self.options,
|
||||
}
|
||||
atomic(record_path / "manifest.json", self.record)
|
||||
try:
|
||||
active = pipeline.start(config, self.callback)
|
||||
self.pipeline = pipeline
|
||||
self.acquisition = "streaming"
|
||||
self.message = ""
|
||||
self.depth_scale = active.get_device().first_depth_sensor().get_depth_scale()
|
||||
calibration = []
|
||||
for p in active.get_streams():
|
||||
if p.is_video_stream_profile():
|
||||
v = p.as_video_stream_profile().get_intrinsics()
|
||||
calibration.append(
|
||||
{
|
||||
"stream": kind(p),
|
||||
"index": p.stream_index(),
|
||||
"width": v.width,
|
||||
"height": v.height,
|
||||
"fx": v.fx,
|
||||
"fy": v.fy,
|
||||
"ppx": v.ppx,
|
||||
"ppy": v.ppy,
|
||||
"coeffs": v.coeffs,
|
||||
"model": str(v.model),
|
||||
}
|
||||
)
|
||||
if self.record:
|
||||
self.record["calibration"] = calibration
|
||||
self.record["depth_scale"] = self.depth_scale
|
||||
atomic(record_path / "manifest.json", self.record)
|
||||
self.thread = threading.Thread(target=self.consume, daemon=True)
|
||||
self.thread.start()
|
||||
except Exception:
|
||||
with suppress(RuntimeError):
|
||||
pipeline.stop()
|
||||
self.pipeline = None
|
||||
self.acquisition = "failed"
|
||||
self.message = "Не удалось открыть потоки камеры. Проверьте подключение и профили."
|
||||
if self.record:
|
||||
self.record.update(state="failed", ended_at=utc())
|
||||
atomic(record_path / "manifest.json", self.record)
|
||||
self.record = None
|
||||
raise ValueError(self.message) from None
|
||||
|
||||
def consume(self):
|
||||
colorizer = rs.colorizer()
|
||||
last_data, last_disk = time.monotonic(), time.monotonic()
|
||||
while not self.stop_event.is_set():
|
||||
try:
|
||||
frames = self.queue.get(timeout=1)
|
||||
except queue.Empty:
|
||||
if time.monotonic() - last_data > 8:
|
||||
self.message = "Кадры перестали поступать. Проверьте USB и остановите захват."
|
||||
self.acquisition = "failed"
|
||||
self.online = False
|
||||
break
|
||||
continue
|
||||
last_data = time.monotonic()
|
||||
for frame in frames:
|
||||
key = kind(frame.profile)
|
||||
if key == "infrared":
|
||||
key += str(frame.profile.stream_index())
|
||||
if not frame.is_video_frame():
|
||||
continue
|
||||
data = np.asanyarray(frame.get_data()).copy()
|
||||
if key == "depth":
|
||||
self.depth = data
|
||||
self.intrinsics = frame.profile.as_video_stream_profile().get_intrinsics()
|
||||
data = np.asanyarray(colorizer.colorize(frame).get_data()).copy()
|
||||
elif data.ndim == 2:
|
||||
data = np.repeat(data[:, :, None], 3, axis=2)
|
||||
elif frame.profile.format() == rs.format.bgr8:
|
||||
data = data[:, :, ::-1].copy()
|
||||
elif frame.profile.format() != rs.format.rgb8:
|
||||
continue
|
||||
self.images[key] = data
|
||||
self.frames[key] = self.frames.get(key, 0) + 1
|
||||
self.last_frame = {
|
||||
"observed_at": utc(),
|
||||
"monotonic_ns": time.monotonic_ns(),
|
||||
"device_timestamp_ms": frame.get_timestamp(),
|
||||
"clock": str(frame.get_frame_timestamp_domain()),
|
||||
}
|
||||
if self.record and time.monotonic() - last_disk > 2:
|
||||
last_disk = time.monotonic()
|
||||
if shutil.disk_usage(self.root).free < 512 * 1024**2:
|
||||
self.message = "Запись остановлена: мало свободного места."
|
||||
break
|
||||
if not self.stop_event.is_set():
|
||||
# Explicit device/disk failure closes raw recording; never auto-restart.
|
||||
self.stop(failed=True, from_capture=True)
|
||||
|
||||
def stop(self, failed=False, from_capture=False):
|
||||
with self.lock:
|
||||
self.stop_event.set()
|
||||
pipeline, self.pipeline = self.pipeline, None
|
||||
if pipeline is not None:
|
||||
self.acquisition = "stopping"
|
||||
pipeline.stop()
|
||||
if self.thread and not from_capture:
|
||||
self.thread.join(timeout=3)
|
||||
self.acquisition = "failed" if failed else "idle"
|
||||
self.revision += 1
|
||||
if self.record:
|
||||
value = dict(self.record)
|
||||
value.update(
|
||||
state="failed" if failed else "complete",
|
||||
ended_at=utc(),
|
||||
ended_monotonic_ns=time.monotonic_ns(),
|
||||
frames=dict(self.frames),
|
||||
)
|
||||
path = self.root / "recordings" / value["id"]
|
||||
source = path / "source.bag"
|
||||
if source.exists():
|
||||
digest = hashlib.sha256()
|
||||
with source.open("rb") as f:
|
||||
for chunk in iter(lambda: f.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
value.update(sha256=digest.hexdigest(), bytes=source.stat().st_size)
|
||||
atomic(path / "manifest.json", value)
|
||||
self.record = None
|
||||
return {"ok": True}
|
||||
|
||||
def verify(self):
|
||||
if self.pipeline is not None:
|
||||
raise ValueError("Остановите захват перед повторной проверкой.")
|
||||
try:
|
||||
self.start()
|
||||
deadline = time.monotonic() + 8
|
||||
expected = {
|
||||
p["stream"] + (str(p["index"]) if p["stream"] == "infrared" else "")
|
||||
for p in self.profiles
|
||||
if p["id"] in self.defaults()
|
||||
}
|
||||
while time.monotonic() < deadline:
|
||||
if all(self.frames.get(key, 0) >= 2 for key in expected):
|
||||
self.config["verified"] = True
|
||||
self.verified_this_process = True
|
||||
self.config["verified_at"] = utc()
|
||||
self.save()
|
||||
return {
|
||||
"ok": True,
|
||||
"frames": dict(self.frames),
|
||||
"verified_at": self.config["verified_at"],
|
||||
}
|
||||
time.sleep(0.1)
|
||||
raise ValueError(
|
||||
"Не получены кадры всех выбранных потоков. Проверьте USB 3 и повторите проверку."
|
||||
)
|
||||
finally:
|
||||
self.stop()
|
||||
|
||||
def rename(self, name):
|
||||
if (
|
||||
not isinstance(name, str)
|
||||
or not name.strip()
|
||||
or len(name) > 80
|
||||
or any(ord(c) < 32 for c in name)
|
||||
):
|
||||
raise ValueError("Введите название до 80 символов.")
|
||||
with self.lock:
|
||||
self.config["name"] = name.strip()
|
||||
self.save()
|
||||
self.revision += 1
|
||||
return {"ok": True}
|
||||
|
||||
def set_option(self, identifier, value):
|
||||
with self.lock:
|
||||
item = next((v for v in self.options if v["id"] == identifier), None)
|
||||
if (
|
||||
not item
|
||||
or item["read_only"]
|
||||
or type(value) not in (int, float)
|
||||
or not math.isfinite(value)
|
||||
or not item["min"] <= value <= item["max"]
|
||||
):
|
||||
raise ValueError("Параметр недоступен или значение вне диапазона.")
|
||||
sensor_index, option_id = map(int, identifier.split(":"))
|
||||
sensor = self.sdk_device.query_sensors()[sensor_index]
|
||||
option = rs.option(option_id)
|
||||
sensor.set_option(option, float(value))
|
||||
item["value"] = sensor.get_option(option)
|
||||
self.revision += 1
|
||||
return {"ok": True, "value": item["value"]}
|
||||
|
||||
def points(self):
|
||||
depth, intrinsics = self.depth, self.intrinsics
|
||||
if depth is None or intrinsics is None:
|
||||
return []
|
||||
# SDK deprojection respects the camera's actual distortion model.
|
||||
h, w = depth.shape
|
||||
stride = max(8, math.ceil(math.sqrt(h * w / 2000)))
|
||||
result = []
|
||||
for y in range(0, h, stride):
|
||||
for x in range(0, w, stride):
|
||||
z = float(depth[y, x]) * self.depth_scale
|
||||
if 0 < z < 15:
|
||||
result.extend(
|
||||
round(v, 4) for v in rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], z)
|
||||
)
|
||||
return result
|
||||
|
||||
def recordings(self):
|
||||
result = []
|
||||
for path in sorted(self.root.glob("recordings/*/manifest.json"), reverse=True)[:100]:
|
||||
value = json.loads(path.read_text())
|
||||
result.append(
|
||||
{
|
||||
k: value.get(k)
|
||||
for k in ("id", "state", "started_at", "ended_at", "bytes", "sha256")
|
||||
}
|
||||
)
|
||||
return result
|
||||
@@ -0,0 +1,126 @@
|
||||
"""Private WebRTC preview. No capture ownership, relay, STUN or public candidates."""
|
||||
|
||||
import asyncio
|
||||
import ipaddress
|
||||
import json
|
||||
import time
|
||||
import uuid
|
||||
|
||||
import aioice.ice
|
||||
from aiortc import RTCConfiguration, RTCPeerConnection, RTCSessionDescription, VideoStreamTrack
|
||||
from av import VideoFrame
|
||||
|
||||
|
||||
def private(address):
|
||||
try:
|
||||
value = ipaddress.ip_address(address)
|
||||
return value.version == 4 and (
|
||||
value.is_loopback
|
||||
or any(
|
||||
value in ipaddress.ip_network(n)
|
||||
for n in ("10.0.0.0/8", "172.16.0.0/12", "192.168.0.0/16", "100.64.0.0/10")
|
||||
)
|
||||
)
|
||||
except ValueError:
|
||||
return False
|
||||
|
||||
|
||||
host_addresses = aioice.ice.get_host_addresses
|
||||
aioice.ice.get_host_addresses = lambda use_ipv4, use_ipv6: [
|
||||
v for v in host_addresses(use_ipv4=True, use_ipv6=False) if private(v)
|
||||
]
|
||||
|
||||
|
||||
class CameraTrack(VideoStreamTrack):
|
||||
def __init__(self, device, layer):
|
||||
super().__init__()
|
||||
self.device, self.layer = device, layer
|
||||
|
||||
async def recv(self):
|
||||
pts, base = await self.next_timestamp()
|
||||
# Bound preview to 15 Hz; hardware profiles and raw recording are independent.
|
||||
await asyncio.sleep(1 / 30)
|
||||
while self.layer not in self.device.images:
|
||||
await asyncio.sleep(0.1)
|
||||
frame = VideoFrame.from_ndarray(self.device.images[self.layer], format="rgb24")
|
||||
frame.pts, frame.time_base = pts, base
|
||||
return frame
|
||||
|
||||
|
||||
class Peers:
|
||||
def __init__(self):
|
||||
self.items = {}
|
||||
|
||||
async def offer(self, device, params):
|
||||
layer = params.get("layer", "color")
|
||||
if layer not in ("color", "depth", "infrared1", "infrared2", "points", "motion"):
|
||||
raise ValueError("Неизвестный слой камеры.")
|
||||
if len(self.items) >= 4:
|
||||
raise ValueError("Закройте лишние окна просмотра камеры.")
|
||||
sdp = params.get("sdp", "")
|
||||
if not isinstance(sdp, str) or len(sdp) > 32768:
|
||||
raise ValueError("Некорректное приглашение просмотра.")
|
||||
for line in sdp.splitlines():
|
||||
if line.startswith("a=candidate:"):
|
||||
fields = line.split()
|
||||
if len(fields) < 8 or (not private(fields[4]) and not fields[4].endswith(".local")):
|
||||
raise ValueError("Просмотр доступен только в частной сети.")
|
||||
pc = RTCPeerConnection(RTCConfiguration(iceServers=[]))
|
||||
ident = "peer_" + uuid.uuid4().hex
|
||||
self.items[ident] = {"pc": pc, "seen": time.monotonic()}
|
||||
|
||||
async def telemetry(channel):
|
||||
try:
|
||||
while pc.connectionState not in ("failed", "closed"):
|
||||
if time.monotonic() - self.items.get(ident, {}).get("seen", 0) > 30:
|
||||
break
|
||||
if channel.readyState == "open" and channel.bufferedAmount < 65536:
|
||||
payload = {
|
||||
"layer": layer,
|
||||
"motion": device.motion,
|
||||
"frame": device.last_frame,
|
||||
"acquisition": device.acquisition,
|
||||
}
|
||||
if layer == "points":
|
||||
payload["points"] = await asyncio.to_thread(device.points)
|
||||
channel.send(json.dumps(payload, allow_nan=False))
|
||||
await asyncio.sleep(0.25)
|
||||
finally:
|
||||
await self.close(ident)
|
||||
|
||||
@pc.on("datachannel")
|
||||
def datachannel(channel):
|
||||
@channel.on("message")
|
||||
def message(value):
|
||||
if value == "keepalive" and ident in self.items:
|
||||
self.items[ident]["seen"] = time.monotonic()
|
||||
|
||||
asyncio.create_task(telemetry(channel))
|
||||
|
||||
@pc.on("connectionstatechange")
|
||||
async def changed():
|
||||
if pc.connectionState in ("failed", "closed"):
|
||||
self.items.pop(ident, None)
|
||||
|
||||
try:
|
||||
await pc.setRemoteDescription(RTCSessionDescription(sdp=sdp, type="offer"))
|
||||
if layer not in ("points", "motion"):
|
||||
pc.addTrack(CameraTrack(device, layer))
|
||||
await pc.setLocalDescription(await pc.createAnswer())
|
||||
|
||||
async def expiry():
|
||||
await asyncio.sleep(30)
|
||||
if ident in self.items and pc.connectionState != "connected":
|
||||
await self.close(ident)
|
||||
|
||||
asyncio.create_task(expiry())
|
||||
return {"peer_id": ident, "sdp": pc.localDescription.sdp, "type": "answer"}
|
||||
except Exception:
|
||||
await self.close(ident)
|
||||
raise
|
||||
|
||||
async def close(self, ident):
|
||||
entry = self.items.pop(ident, None)
|
||||
if entry:
|
||||
await entry["pc"].close()
|
||||
return {"ok": True}
|
||||
@@ -0,0 +1,161 @@
|
||||
"""Private Unix plugin host; the Go broker is the only UI/Core authority."""
|
||||
|
||||
import asyncio
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import uuid
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
|
||||
import pyrealsense2 as rs
|
||||
from aiohttp import web
|
||||
from device import Device, atomic, device_id
|
||||
from media import Peers
|
||||
from missioncore_plugin_sdk.v0alpha2.operations import OperationRequest
|
||||
|
||||
ROOT = Path("/var/lib/mission-core-sensors")
|
||||
SOCKET = Path("/run/mission-core-sensors/driver.sock")
|
||||
|
||||
|
||||
class Host:
|
||||
def __init__(self, root=ROOT):
|
||||
self.root = root
|
||||
self.context = rs.context()
|
||||
self.devices = {}
|
||||
self.execution = None
|
||||
self.peers = Peers()
|
||||
self.scan_lock = asyncio.Lock()
|
||||
self.operation_locks = {}
|
||||
|
||||
async def scan(self):
|
||||
if self.execution is None:
|
||||
return
|
||||
async with self.scan_lock:
|
||||
|
||||
def work():
|
||||
found = set()
|
||||
for dev in self.context.query_devices():
|
||||
if dev.get_info(rs.camera_info.product_id).lower() != "0b5c":
|
||||
continue
|
||||
serial = dev.get_info(rs.camera_info.serial_number)
|
||||
ident = device_id(serial)
|
||||
found.add(ident)
|
||||
if ident not in self.devices:
|
||||
self.devices[ident] = Device(serial, self.root, self.execution)
|
||||
self.devices[ident].refresh(dev)
|
||||
for ident, device in self.devices.items():
|
||||
if ident not in found:
|
||||
device.online = False
|
||||
|
||||
await asyncio.to_thread(work)
|
||||
|
||||
async def inventory(self, request):
|
||||
node_id = request.headers.get("X-Node-Id", "")
|
||||
if not re.fullmatch(r"node_[0-9a-f]{64}", node_id):
|
||||
raise web.HTTPForbidden()
|
||||
if self.execution is None:
|
||||
self.execution = {
|
||||
"node_id": node_id,
|
||||
"agent_instance_id": "driver_" + uuid.uuid4().hex,
|
||||
"platform": "linux",
|
||||
}
|
||||
if self.execution["node_id"] != node_id:
|
||||
raise web.HTTPForbidden()
|
||||
await self.scan()
|
||||
return web.json_response(
|
||||
{"items": [device.snapshot(False) for device in self.devices.values()]}
|
||||
)
|
||||
|
||||
async def operation(self, request):
|
||||
value = await request.json()
|
||||
command = OperationRequest.model_validate(value)
|
||||
identifier = command.operation_id
|
||||
if not re.fullmatch(r"op_[0-9a-f]{32}", identifier):
|
||||
raise ValueError("Некорректный идентификатор операции.")
|
||||
path = self.root / (identifier + ".json")
|
||||
digest = hashlib.sha256(json.dumps(value, sort_keys=True).encode()).hexdigest()
|
||||
lock = self.operation_locks.setdefault(identifier, asyncio.Lock())
|
||||
async with lock:
|
||||
if path.exists():
|
||||
previous = json.loads(path.read_text())
|
||||
if previous["digest"] != digest:
|
||||
raise ValueError("Идентификатор операции уже использован.")
|
||||
return web.json_response(previous["result"])
|
||||
if command.deadline_at <= datetime.now(UTC):
|
||||
raise ValueError("Срок команды истёк. Состояние камеры не изменено.")
|
||||
device = self.devices.get(command.session.device_id)
|
||||
if device is None or command.session.session_id != device.session:
|
||||
raise ValueError("Сеанс камеры изменился. Обновите сведения.")
|
||||
# Persist uncertainty before any side effect. A crash must not replay START.
|
||||
receipt = {
|
||||
"digest": digest,
|
||||
"result": {
|
||||
"state": "unknown",
|
||||
"error": "Результат операции пока неизвестен. Обновите состояние устройства.",
|
||||
},
|
||||
}
|
||||
atomic(path, receipt)
|
||||
try:
|
||||
action, params = command.action_id, dict(command.parameters)
|
||||
if action == "details":
|
||||
result = device.snapshot()
|
||||
elif action == "rename":
|
||||
result = device.rename(params.get("name"))
|
||||
elif action == "verify":
|
||||
result = await asyncio.to_thread(device.verify)
|
||||
elif action == "start":
|
||||
if type(params.get("record", False)) is not bool:
|
||||
raise ValueError("Некорректный режим записи.")
|
||||
await asyncio.to_thread(
|
||||
device.start, params.get("profiles"), params.get("record", False)
|
||||
)
|
||||
result = {"ok": True}
|
||||
elif action == "stop":
|
||||
result = await asyncio.to_thread(device.stop)
|
||||
elif action == "option":
|
||||
result = await asyncio.to_thread(
|
||||
device.set_option, params.get("id"), params.get("value")
|
||||
)
|
||||
elif action == "offer":
|
||||
result = await self.peers.offer(device, params)
|
||||
elif action == "close-peer":
|
||||
result = await self.peers.close(params.get("peer_id"))
|
||||
else:
|
||||
raise ValueError("Неподдерживаемая операция устройства.")
|
||||
receipt["result"] = {"state": "complete", "result": result}
|
||||
except (ValueError, RuntimeError) as error:
|
||||
receipt["result"] = {"state": "error", "error": str(error)[:400]}
|
||||
atomic(path, receipt)
|
||||
return web.json_response(receipt["result"])
|
||||
|
||||
async def cleanup(self, app):
|
||||
for peer in list(self.peers.items):
|
||||
await self.peers.close(peer)
|
||||
for device in self.devices.values():
|
||||
await asyncio.to_thread(device.stop, True)
|
||||
|
||||
|
||||
@web.middleware
|
||||
async def errors(request, handler):
|
||||
try:
|
||||
return await handler(request)
|
||||
except (ValueError, KeyError, TypeError):
|
||||
return web.json_response({"error": "Некорректная команда устройства."}, status=400)
|
||||
except RuntimeError:
|
||||
return web.json_response(
|
||||
{"error": "Драйвер не смог выполнить запрос. Проверьте подключение камеры."}, status=409
|
||||
)
|
||||
|
||||
|
||||
def main():
|
||||
os.umask(0o007)
|
||||
host = Host()
|
||||
app = web.Application(client_max_size=65536, middlewares=[errors])
|
||||
app.router.add_get("/inventory", host.inventory)
|
||||
app.router.add_post("/operation", host.operation)
|
||||
app.on_cleanup.append(host.cleanup)
|
||||
if SOCKET.exists():
|
||||
SOCKET.unlink()
|
||||
web.run_app(app, path=str(SOCKET), print=None, access_log=None, shutdown_timeout=20)
|
||||
Reference in New Issue
Block a user