Add D455 sensor host and shared Node/Core preparation surface
This commit is contained in:
DCCONSTRUCTIONS
@@ -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
|
||||
Reference in New Issue
Block a user