fix(node): verify D455 access and share sensor progress and recording UI

apps/node-agent/sensors/device.py

@@ -2,9 +2,11 @@
 
 import hashlib
 import json
+import logging
 import math
 import os
 import queue
+import re
 import shutil
 import threading
 import time
@@ -18,7 +20,7 @@ from missioncore_plugin_sdk.v0alpha2.session import DeviceSessionSnapshot
 
 MODEL = {
     "plugin_id": "missioncore.realsense",
-    "plugin_version": "0.6.0",
+    "plugin_version": "0.6.6",
     "model_id": "realsense.d455",
 }
 
@@ -44,10 +46,19 @@ 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):
+    def __init__(self, serial, root, execution, usb_serial=None):
         self.serial = serial
-        self.id = device_id(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()
@@ -69,6 +80,7 @@ class Device:
         self.frames = {}
         self.last_frame = None
         self.record = None
+        self.playback_id = None
         self.profiles = []
         self.options = []
         self.sdk_device = None
@@ -91,6 +103,10 @@ class Device:
     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)
@@ -204,6 +220,7 @@ class Device:
                 "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 []),
@@ -253,15 +270,11 @@ class Device:
                 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"])
+            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 = {}, {}, {}
@@ -276,7 +289,7 @@ class Device:
                 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"))
+                config.enable_record_to_file(str(record_path / "source.db3"))
                 self.record = {
                     "id": ident,
                     "state": "recording",
@@ -287,6 +300,7 @@ class Device:
                     "profiles": profiles,
                     "firmware": self.firmware,
                     "sdk": "2.58.4.10922",
+                    "storage_format": "rosbag2-sqlite3",
                     "options": self.options,
                 }
                 atomic(record_path / "manifest.json", self.record)
@@ -320,7 +334,8 @@ class Device:
                     atomic(record_path / "manifest.json", self.record)
                 self.thread = threading.Thread(target=self.consume, daemon=True)
                 self.thread.start()
-            except Exception:
+            except Exception as error:
+                logging.error("D455 capture: %s", str(error).replace(self.serial, "[camera]"))
                 with suppress(RuntimeError):
                     pipeline.stop()
                 self.pipeline = None
@@ -332,6 +347,49 @@ class Device:
                     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:
+                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()
@@ -387,8 +445,10 @@ class Device:
             if pipeline is not None:
                 self.acquisition = "stopping"
                 pipeline.stop()
+                del pipeline
             if self.thread and not from_capture:
                 self.thread.join(timeout=3)
+            self.playback_id = None
             self.acquisition = "failed" if failed else "idle"
             self.revision += 1
             if self.record:
@@ -400,7 +460,7 @@ class Device:
                     frames=dict(self.frames),
                 )
                 path = self.root / "recordings" / value["id"]
-                source = path / "source.bag"
+                source = path / "source.db3"
                 if source.exists():
                     digest = hashlib.sha256()
                     with source.open("rb") as f:
@@ -465,6 +525,8 @@ class Device:
                 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)

apps/node-agent/sensors/media.py

@@ -5,6 +5,7 @@ import ipaddress
 import json
 import time
 import uuid
+from fractions import Fraction
 
 import aioice.ice
 from aiortc import RTCConfiguration, RTCPeerConnection, RTCSessionDescription, VideoStreamTrack
@@ -35,11 +36,16 @@ class CameraTrack(VideoStreamTrack):
     def __init__(self, device, layer):
         super().__init__()
         self.device, self.layer = device, layer
+        self.started = None
+        self.sequence = 0
 
     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)
+        # Fixed 15 Hz preview clock; raw hardware timing is independent.
+        if self.started is None:
+            self.started = time.monotonic()
+        await asyncio.sleep(max(0, self.started + self.sequence / 15 - time.monotonic()))
+        pts, base = self.sequence * 6000, Fraction(1, 90000)
+        self.sequence += 1
         while self.layer not in self.device.images:
             await asyncio.sleep(0.1)
         frame = VideoFrame.from_ndarray(self.device.images[self.layer], format="rgb24")

apps/node-agent/sensors/server.py

@@ -37,17 +37,35 @@ class Host:
             def work():
                 found = set()
                 for dev in self.context.query_devices():
+                    if dev.is_playback():
+                        continue
                     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)
+                    # SDK module serial and USB serial are distinct on D455.
+                    # Resolve the actual transport ancestor, not list order or model count.
+                    physical = Path(dev.get_info(rs.camera_info.physical_port)).resolve()
+                    usb_serial = None
+                    for parent in (physical, *physical.parents):
+                        if (
+                            (parent / "idVendor").exists()
+                            and (parent / "idProduct").exists()
+                            and (parent / "idVendor").read_text().strip() == "8086"
+                            and (parent / "idProduct").read_text().strip() == "0b5c"
+                        ):
+                            usb_serial = (parent / "serial").read_text().strip()
+                            break
+                    if not usb_serial:
+                        continue
+                    ident = device_id(usb_serial)
                     found.add(ident)
                     if ident not in self.devices:
-                        self.devices[ident] = Device(serial, self.root, self.execution)
+                        self.devices[ident] = Device(serial, self.root, self.execution, usb_serial)
                     self.devices[ident].refresh(dev)
                 for ident, device in self.devices.items():
                     if ident not in found:
                         device.online = False
+                        device.verified_this_process = False
 
             await asyncio.to_thread(work)
 
@@ -112,6 +130,8 @@ class Host:
                         device.start, params.get("profiles"), params.get("record", False)
                     )
                     result = {"ok": True}
+                elif action == "replay":
+                    result = await asyncio.to_thread(device.replay, params.get("recording_id"))
                 elif action == "stop":
                     result = await asyncio.to_thread(device.stop)
                 elif action == "option":
@@ -128,8 +148,20 @@ class Host:
             except (ValueError, RuntimeError) as error:
                 receipt["result"] = {"state": "error", "error": str(error)[:400]}
             atomic(path, receipt)
+            self.operation_locks.pop(identifier, None)
             return web.json_response(receipt["result"])
 
+    async def prepare_safe(self, request):
+        return web.json_response(
+            {
+                "safe": all(
+                    d.pipeline is None
+                    and d.acquisition not in ("preparing", "starting", "stopping")
+                    for d in self.devices.values()
+                )
+            }
+        )
+
     async def cleanup(self, app):
         for peer in list(self.peers.items):
             await self.peers.close(peer)
@@ -154,6 +186,7 @@ def main():
     host = Host()
     app = web.Application(client_max_size=65536, middlewares=[errors])
     app.router.add_get("/inventory", host.inventory)
+    app.router.add_get("/prepare-safe", host.prepare_safe)
     app.router.add_post("/operation", host.operation)
     app.on_cleanup.append(host.cleanup)
     if SOCKET.exists():