608 lines
26 KiB
Python
608 lines
26 KiB
Python
"""D455 adapter. Hardware ownership and raw acquisition remain on the board."""
|
|
|
|
import hashlib
|
|
import json
|
|
import logging
|
|
import math
|
|
import os
|
|
import queue
|
|
import re
|
|
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.6",
|
|
"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]
|
|
|
|
|
|
def configure_profiles(config, profiles):
|
|
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"])
|
|
|
|
|
|
class Device:
|
|
def __init__(self, serial, root, execution, usb_serial=None):
|
|
self.serial = serial
|
|
self.id = device_id(usb_serial or 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.playback_id = 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") in ("recording", "finalizing"):
|
|
value.update(state="interrupted", recovered_at=utc())
|
|
atomic(path, value)
|
|
|
|
def save(self):
|
|
atomic(self.root / "config.json", self.config)
|
|
|
|
def disconnect(self):
|
|
with self.lock:
|
|
if not self.online:
|
|
return
|
|
self.online = False
|
|
self.verified_this_process = False
|
|
self.sdk_device = None
|
|
# Stored playback is independent from a physical USB camera.
|
|
live = self.pipeline is not None and self.playback_id is None
|
|
if self.playback_id is None:
|
|
self.images = {}
|
|
self.motion = {}
|
|
self.depth = None
|
|
self.message = "Камера отключена от БК."
|
|
if live:
|
|
self.stop(failed=True)
|
|
|
|
def refresh(self, dev):
|
|
with self.lock:
|
|
self.sdk_device = dev
|
|
if not self.online:
|
|
self.verified_this_process = False
|
|
self.session = "sensor_" + uuid.uuid4().hex
|
|
self.opened = utc()
|
|
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 or self.acquisition == "stopping":
|
|
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,
|
|
"playback_id": self.playback_id,
|
|
"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 or self.acquisition == "stopping":
|
|
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)
|
|
configure_profiles(config, profiles)
|
|
pipeline = rs.pipeline()
|
|
if not config.can_resolve(rs.pipeline_wrapper(pipeline)):
|
|
raise ValueError("Камера не поддерживает эту комбинацию профилей. Выберите другую.")
|
|
self.playback_id = None
|
|
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.db3"))
|
|
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",
|
|
"storage_format": "rosbag2-sqlite3",
|
|
"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 as error:
|
|
logging.error("D455 capture: %s", str(error).replace(self.serial, "[camera]"))
|
|
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 replay(self, ident):
|
|
# Only completed board-owned recordings; UI never supplies a filesystem path.
|
|
if not isinstance(ident, str) or not re.fullmatch(r"capture_[0-9a-f]{32}", ident):
|
|
raise ValueError("Некорректная запись.")
|
|
with self.lock:
|
|
if self.pipeline is not None or self.acquisition == "stopping":
|
|
raise ValueError("Сначала остановите текущий захват или просмотр записи.")
|
|
directory = self.root / "recordings" / ident
|
|
source, manifest = directory / "source.db3", directory / "manifest.json"
|
|
if directory.is_symlink() or source.is_symlink() or manifest.is_symlink():
|
|
raise ValueError("Запись недоступна.")
|
|
if not source.is_file() or not manifest.is_file():
|
|
raise ValueError("Запись не найдена.")
|
|
value = json.loads(manifest.read_text())
|
|
if value.get("state") != "complete" or source.stat().st_size != value.get("bytes"):
|
|
raise ValueError("Запись не завершена или повреждена.")
|
|
config, pipeline = rs.config(), rs.pipeline()
|
|
config.enable_device_from_file(str(source), repeat_playback=True)
|
|
configure_profiles(config, value["profiles"])
|
|
self.images, self.motion, self.frames = {}, {}, {}
|
|
self.depth, self.last_frame = None, None
|
|
self.queue = queue.Queue(maxsize=2)
|
|
self.stop_event.clear()
|
|
self.acquisition = "starting"
|
|
try:
|
|
active = pipeline.start(config, self.callback)
|
|
self.pipeline = pipeline
|
|
active.get_device().as_playback().set_real_time(True)
|
|
self.depth_scale = active.get_device().first_depth_sensor().get_depth_scale()
|
|
self.playback_id = ident
|
|
self.acquisition = "streaming"
|
|
self.message = ""
|
|
self.thread = threading.Thread(target=self.consume, daemon=True)
|
|
self.thread.start()
|
|
except RuntimeError:
|
|
with suppress(RuntimeError):
|
|
pipeline.stop()
|
|
self.pipeline = None
|
|
self.acquisition = "failed"
|
|
raise ValueError("Не удалось открыть исходную запись.") from None
|
|
self.revision += 1
|
|
return {"ok": True, "playback_id": ident}
|
|
|
|
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:
|
|
if self.acquisition == "stopping":
|
|
raise ValueError("Исходная запись ещё сохраняется. Дождитесь завершения.")
|
|
self.stop_event.set()
|
|
pipeline, self.pipeline = self.pipeline, None
|
|
if pipeline is not None:
|
|
self.acquisition = "stopping"
|
|
try:
|
|
pipeline.stop()
|
|
except RuntimeError:
|
|
# Removal may invalidate the SDK handle before STOP reaches it.
|
|
failed = True
|
|
self.message = "Захват прерван. Проверьте подключение камеры."
|
|
del pipeline
|
|
if self.thread and not from_capture:
|
|
self.thread.join(timeout=3)
|
|
self.playback_id = None
|
|
value = dict(self.record) if self.record else None
|
|
self.acquisition = "stopping" if value else "failed" if failed else "idle"
|
|
self.revision += 1
|
|
if value:
|
|
value.update(
|
|
state="finalizing",
|
|
ended_at=utc(),
|
|
ended_monotonic_ns=time.monotonic_ns(),
|
|
frames=dict(self.frames),
|
|
)
|
|
self.record = value
|
|
path = self.root / "recordings" / value["id"]
|
|
atomic(path / "manifest.json", value)
|
|
if value:
|
|
# Hashing a large source must not lock inventory or heartbeat. The
|
|
# stopping state still prevents START, preparation and package updates.
|
|
try:
|
|
source = path / "source.db3"
|
|
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)
|
|
else:
|
|
failed = True
|
|
value["state"] = "failed" if failed else "complete"
|
|
atomic(path / "manifest.json", value)
|
|
except OSError:
|
|
failed = True
|
|
self.message = "Не удалось завершить сохранение исходной записи. Проверьте диск БК."
|
|
value["state"] = "failed"
|
|
atomic(path / "manifest.json", value)
|
|
finally:
|
|
with self.lock:
|
|
self.record = None
|
|
self.acquisition = "failed" if failed else "idle"
|
|
self.revision += 1
|
|
return {"ok": True}
|
|
|
|
def verify(self):
|
|
if self.pipeline is not None or self.acquisition == "stopping":
|
|
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("Параметр недоступен или значение вне диапазона.")
|
|
if self.playback_id:
|
|
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
|