ROS-узел, устойчивость к битому CDR, гейт регрессии полигона

Узел tools/flyguard_ros2_node.py (п. 16.7: отдавал массив вместо PointCloud2,
публиковал треки без решения) переписан: разбор через новый
flyguard.cdr.from_ros_message, публикация через flyguard.export.export_frame,
не падает от битого кадра, по умолчанию подключает artifacts/*.npz.

flyguard/bag.py: битый CDR-пакет в бэге пропускается с логом вместо обрыва
всего чтения (Bag.n_frames_failed). tools/compare_benchmark.py: флаг
--fail-on-net-down для CI-гейта регрессии по методологии парных переворотов
(EXPERIMENTS п. 16.1). Добавлен pyproject.toml (ruff, не прогнан — в
окружении нет ruff/pip). Тесты 46/46 (tests/test_pipeline.py).

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

docs/EXPERIMENTS.md

@@ -2707,3 +2707,69 @@ SIGTERM — узел его переживал. Узлы копились: на
 нужно смотреть глазами: по словам организаторов, препятствий в данных «не
 больше двух», и одно из них в `doubleT_obstacle` — второе может оказаться
 здесь.
+
+## 18. Узел `tools/flyguard_ros2_node.py` доведён до рабочего состояния
+
+П. 16.7 фиксировал узел как нерабочий («отдаёт конвейеру массив numpy вместо
+разобранного `PointCloud2`, публикует все треки, а не решение, и у трека нет
+полей рамки») и оставлял его за интеграцией. Отдельного `ros2_ws/.../node.py`
+или `export_team.py` в репозитории нет — это единственный узел, поэтому
+переписан начисто:
+
+* **Вход.** [`flyguard/cdr.py`](../flyguard/cdr.py) получил `from_ros_message`:
+  переводит уже разобранное rclpy-сообщение в тот же `PointCloud2`, что и
+  офлайн-парсер, по общему `point_dtype`. Порядок точек и NaN сохраняются —
+  `sensor_msgs_py.read_points(skip_nans=True)` их выбрасывал, а решётка
+  `retina.py` раскладывает облако по позиции в массиве (азимут × кольцо), и
+  выброшенная точка сдвигала бы всю решётку. Тест —
+  `test_ros_message_keeps_every_point_in_order`.
+* **Решение, не треки.** Узел зовёт [`flyguard/export.py`](../flyguard/export.py)
+  (`export_frame`) — тот же модуль, что уже решал разворот рамки в кривой и
+  зеркальный `yaw` (п. 16.7) — и публикует `Decision`, а не сырые треки
+  `CentralComplex`. Рамки идут и в `vision_msgs/Detection3DArray`, и в
+  `visualization_msgs/MarkerArray` (кубы + подписи «ID | дальность | TTC»), из
+  одного и того же `ExportResult`, так что они не могут разойтись.
+* **Не падает от битого кадра.** `pointcloud_callback` оборачивает
+  `fg.process()` в `try/except`: следующий кадр придёт через 100 мс, а не
+  превратит узел в мёртвый процесс на записи с реальным Pandar128E3X, где
+  структура сообщения может отличаться от тестовых бэгов.
+* **Память и считывания подключаются по умолчанию.** Раньше узел собирал
+  `FlyGuard` без `memory`/`readout` вообще — конвейер работал в режиме «всё
+  ново», без обученной памяти тоннеля. Теперь по умолчанию берутся
+  `artifacts/mushroom_body.npz` и `artifacts/mbon_readout.npz`, если они есть
+  (параметры `memory_path`/`readout_path` позволяют переопределить путь или
+  явно отключить значением `-`).
+* **Устройство по умолчанию — `cpu`**, как и решил п. 16.7: на GPU
+  ускоряется только `Lamina`, а `device=auto` в узле интеграции — источник
+  тихих ошибок, если жюри не даёт GPU.
+
+Не проверено на самом Pandar128E3X и не прогнано в реальном `ros2_ws` — только
+модульным тестом и прогоном логики колбэка на заглушках `rclpy`/`vision_msgs`
+(тестовое окружение без ROS2). Перед демо нужен прогон `ros2 bag play` живой
+записи с этим узлом.
+
+## 19. `flyguard/bag.py`: битый CDR-пакет больше не обрывает чтение бэга
+
+`Bag.frames()` звал `parse_pointcloud2(blob)` без защиты — одно повреждённое
+сообщение (реальный сенсор, не тестовый бэг) роняло исключение наружу и
+обрывало генератор, а с ним — весь прогон `tools/evaluate.py` /
+`tools/run_pipeline.py` на этой записи, вместе со всеми уже накопленными
+кадрами. Теперь разбор каждого сообщения обёрнут в `try/except`: битый кадр
+логируется (`flyguard.bag`, номер кадра, файл шарда, тип исключения) и
+пропускается, `start`/`stop` по-прежнему считают по позиции сообщения в
+бэге, а не по числу успешно разобранных — окно `frames(start=..., stop=...)`
+не съезжает. Число пропусков доступно как `Bag.n_frames_failed` после
+прохода. Тест — `test_bag_skips_a_corrupted_message_and_keeps_reading`
+(набор вырос до 46/46), с отдельным минимальным CDR-кодировщиком в
+`tests/test_pipeline.py` для валидного `PointCloud2` без зависимости от
+реальных бэгов.
+
+Два других пункта из того же списка критики остаются открытыми — их нельзя
+закрыть в этом окружении:
+
+* **Три неопознанных ложных трека (п. 17.7)** требуют визуального разбора по
+  самим бэгам/RViz — самих файлов записей в `data/` в этой рабочей копии нет.
+* **ROS2-узел на реальном стенде** — в этом окружении не установлен пакет
+  `rclpy` (`ModuleNotFoundError: No module named 'rclpy'`), так что живой
+  прогон `ros2 bag play` с узлом (п. 18) не выполним здесь; логика узла
+  проверена только на заглушках.

flyguard/bag.py

@@ -6,6 +6,7 @@
 """
 from __future__ import annotations
 
+import logging
 import re
 import sqlite3
 from contextlib import closing
@@ -15,6 +16,8 @@ from typing import Iterator
 
 from .cdr import PointCloud2, parse_pointcloud2
 
+logger = logging.getLogger("flyguard.bag")
+
 _SHARD_RE = re.compile(r"_(\d+)\.db3$")
 _POINTCLOUD_TYPE = "sensor_msgs/msg/PointCloud2"
 
@@ -44,6 +47,7 @@ class Bag:
         self.shards = shards
         self.topics = self._scan_topics()
         self.topic = topic or self._pick_topic()
+        self.n_frames_failed = 0  # обновляется по ходу последнего frames()
 
     @staticmethod
     def _shard_key(p: Path) -> tuple[int, str]:
@@ -83,9 +87,13 @@ class Bag:
         """Выдать (timestamp_ns, облако) для сообщений выбранного топика.
 
         Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
-        шарда — по возрастанию времени.
+        шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать
+        (битый CDR у реального сенсора), пропускается с предупреждением в лог,
+        а не останавливает чтение бэга — `start`/`stop` считают по позиции
+        сообщения в бэге, а не по числу успешно разобранных.
         """
         idx = 0
+        self.n_frames_failed = 0
         for shard in self.shards:
             # closing(), а не сам connection: у sqlite3 `with` управляет
             # транзакцией и файл остаётся открытым — на Windows его потом
@@ -111,7 +119,15 @@ class Bag:
                     if stop is not None and idx >= stop:
                         return
                     if idx >= start and (idx - start) % stride == 0:
-                        yield ts, parse_pointcloud2(blob)
+                        try:
+                            pc = parse_pointcloud2(blob)
+                        except Exception as e:  # noqa: BLE001
+                            self.n_frames_failed += 1
+                            logger.warning(
+                                "кадр %d в %s не разобран (%s: %s) — пропущен",
+                                idx, shard.name, type(e).__name__, e)
+                        else:
+                            yield ts, pc
                     idx += 1
 
     def describe(self) -> str:

flyguard/cdr.py

@@ -4,7 +4,8 @@
   * офлайн-эксперименты на машине без ROS (Windows);
   * прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
 
-Внутри ROS-ноды сообщение приходит уже разобранным, и этот модуль не используется.
+Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же
+`PointCloud2` функция `from_ros_message`.
 """
 from __future__ import annotations
 
@@ -128,3 +129,21 @@ def parse_pointcloud2(blob: bytes | memoryview) -> PointCloud2:
     return PointCloud2(stamp=sec + nsec * 1e-9, frame_id=frame_id, height=height,
                        width=width, point_step=point_step, is_dense=is_dense,
                        points=points)
+
+
+def from_ros_message(msg) -> PointCloud2:
+    """Перевести разобранное rclpy-сообщение sensor_msgs/msg/PointCloud2.
+
+    Все height·width точек сохраняются, включая NaN: сетчатка раскладывает облако
+    в решётку азимут × кольцо по порядку точек, и выброшенная точка сдвинула бы
+    всю решётку (`sensor_msgs_py.read_points(skip_nans=True)` делает именно это).
+    """
+    if msg.is_bigendian:
+        raise ValueError("big-endian PointCloud2 не поддерживается")
+    fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
+    dt = point_dtype(fields, msg.point_step)
+    points = np.frombuffer(msg.data, dtype=dt, count=msg.height * msg.width)
+    stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+    return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id, height=msg.height,
+                       width=msg.width, point_step=msg.point_step,
+                       is_dense=bool(msg.is_dense), points=points)

pyproject.toml

@@ -0,0 +1,13 @@
+[tool.ruff]
+target-version = "py311"
+line-length = 96
+extend-exclude = ["artifacts", "data", "__pycache__"]
+
+[tool.ruff.lint]
+select = ["E", "F", "W", "B", "UP", "SIM"]
+ignore = [
+    "E501",   # длину строк проверяет line-length, но многодетные формулы иногда шире
+]
+
+[tool.ruff.lint.per-file-ignores]
+"tests/*" = ["E402"]  # sys.path.insert перед импортом flyguard — так во всех тестах

tests/test_pipeline.py

@@ -8,6 +8,8 @@
 """
 from __future__ import annotations
 
+import sqlite3
+import struct
 import sys
 from pathlib import Path
 
@@ -50,6 +52,37 @@ def test_point_dtype_rejects_overlapping_fields():
         point_dtype([("x", 0, 8, 1), ("y", 4, 7, 1)], 26)
 
 
+def test_ros_message_keeps_every_point_in_order():
+    """Узел ROS: NaN-точки остаются на месте, иначе решётка азимут × кольцо съедет."""
+    from types import SimpleNamespace as NS
+
+    from flyguard.cdr import from_ros_message
+
+    spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1),
+            ("ring", 16, 4, 1), ("timestamp", 18, 8, 1)]
+    dt = point_dtype(spec, 26)
+    n_rings, n_cols = 4, 5
+    pts = np.zeros(n_rings * n_cols, dt)
+    pts["x"] = np.arange(pts.size, dtype=np.float32)
+    pts["ring"] = np.tile(np.arange(n_rings), n_cols)
+    pts["timestamp"] = 1.5 + np.arange(pts.size) * 1e-6
+    pts["x"][[3, 7]] = np.nan
+    msg = NS(header=NS(stamp=NS(sec=12, nanosec=500_000_000), frame_id="hesai_lidar"),
+             height=1, width=pts.size, point_step=26, is_bigendian=False, is_dense=False,
+             fields=[NS(name=n, offset=o, datatype=t, count=c) for n, o, t, c in spec],
+             data=pts.tobytes())
+
+    pc = from_ros_message(msg)
+
+    assert pc.n_points == n_rings * n_cols
+    assert pc.stamp == pytest.approx(12.5)
+    assert pc.frame_id == "hesai_lidar"
+    assert np.isnan(pc.points["x"][[3, 7]]).all()
+    assert pc.points["x"][8] == 8.0
+    assert np.array_equal(pc.points["ring"], pts["ring"])
+    assert np.array_equal(pc.points["timestamp"], pts["timestamp"])
+
+
 # --------------------------------------------------------------------------- решётка
 
 def _layout(n_rings=8, n_az=40, n_echo=2, shift=None):
@@ -389,6 +422,64 @@ def test_injected_intensity_copies_the_surroundings():
 
 # --------------------------------------------------------------------------- данные
 
+def _write_minimal_bag(db_path, blobs: list[bytes]):
+    """Собрать минимальный `.db3` со схемой rosbag2, нужной `flyguard.bag.Bag`."""
+    con = sqlite3.connect(db_path)
+    con.execute("CREATE TABLE topics (id INTEGER PRIMARY KEY, name TEXT, type TEXT)")
+    con.execute("CREATE TABLE messages (id INTEGER PRIMARY KEY, topic_id INTEGER, "
+                "timestamp INTEGER, data BLOB)")
+    con.execute("INSERT INTO topics VALUES (1, '/lidar_points', "
+                "'sensor_msgs/msg/PointCloud2')")
+    for i, blob in enumerate(blobs):
+        con.execute("INSERT INTO messages (topic_id, timestamp, data) VALUES (1, ?, ?)",
+                    (i, blob))
+    con.commit()
+    con.close()
+
+
+def _cdr_pointcloud2_blob(n_points: int) -> bytes:
+    """Валидный CDR sensor_msgs/msg/PointCloud2: один float32 `x` на точку."""
+    def s(text: str) -> bytes:
+        b = text.encode() + b"\x00"
+        pad = (-len(b)) % 4
+        return struct.pack("<I", len(b)) + b + b"\x00" * pad
+
+    buf = bytearray(b"\x00\x00\x00\x00")  # заголовок инкапсуляции CDR
+    buf += struct.pack("<ii", 0, 0)        # header.stamp (sec, nanosec)
+    buf += s("hesai_lidar")                # header.frame_id
+    buf += struct.pack("<II", 1, n_points)  # height, width
+    buf += struct.pack("<I", 1)            # число полей
+    buf += s("x")
+    buf += struct.pack("<I", 0)            # offset
+    buf += struct.pack("<B", 7)            # datatype=FLOAT32
+    buf += b"\x00\x00\x00"                 # паддинг octet -> uint32 (_align(4))
+    buf += struct.pack("<I", 1)            # count
+    buf += struct.pack("<B", 0)            # is_bigendian
+    buf += b"\x00\x00\x00"                 # паддинг octet -> uint32 (_align(4))
+    buf += struct.pack("<I", 4)            # point_step
+    buf += struct.pack("<I", 4 * n_points)  # row_step
+    data = np.arange(n_points, dtype="<f4").tobytes()
+    buf += struct.pack("<I", len(data)) + data
+    buf += struct.pack("<B", 1)            # is_dense
+    return bytes(buf)
+
+
+def test_bag_skips_a_corrupted_message_and_keeps_reading(tmp_path):
+    """Один битый CDR-пакет (реальный сенсор) не должен обрывать чтение бэга."""
+    from flyguard.bag import Bag
+
+    good_a = _cdr_pointcloud2_blob(5)
+    good_b = _cdr_pointcloud2_blob(7)
+    broken = b"\x00\x00\x00\x00" + b"\xff" * 3  # обрывается на первом же string()
+    _write_minimal_bag(tmp_path / "chunk_0.db3", [good_a, broken, good_b])
+
+    bag = Bag(tmp_path)
+    frames = list(bag.frames())
+
+    assert [pc.n_points for _, pc in frames] == [5, 7]
+    assert bag.n_frames_failed == 1
+
+
 @pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
 def test_real_bag_projects_without_angular_error():
     """На реальном бэге выпрямленная решётка обязана описывать лучи точно."""

tools/compare_benchmark.py

@@ -14,6 +14,10 @@
 
     python tools/compare_benchmark.py artifacts/benchmark.json new.json
     python tools/compare_benchmark.py a.json b.json --objects человек_лежа,ящик
+
+Для CI-гейта (провалить сборку при регрессии) добавьте
+`--fail-on-net-down N`: выход с кодом 1, если (1→0) − (0→1) по всем
+предметам больше N. Не включено по умолчанию — без него это только отчёт.
 """
 from __future__ import annotations
 
@@ -38,6 +42,8 @@ def main() -> None:
                     help="через запятую; пусто — все предметы")
     ap.add_argument("--edges", default="0,15,40,90,130,170,230",
                     help="границы полос дальности, м")
+    ap.add_argument("--fail-on-net-down", type=int, default=None, metavar="N",
+                    help="выйти с кодом 1, если (1→0) − (0→1) больше N (для CI)")
     args = ap.parse_args()
 
     a, b = load(args.before), load(args.after)
@@ -78,6 +84,12 @@ def main() -> None:
         total_down += obj_down
     print(f"\nвсе предметы: 0→1 {total_up}, 1→0 {total_down}")
 
+    if args.fail_on_net_down is not None:
+        net_down = total_down - total_up
+        if net_down > args.fail_on_net_down:
+            raise SystemExit(f"регрессия: (1→0) − (0→1) = {net_down} > "
+                             f"{args.fail_on_net_down}")
+
 
 if __name__ == "__main__":
     main()

tools/flyguard_ros2_node.py

@@ -1,13 +1,21 @@
 """
 ROS 2 узел для биоинспирированного пайплайна FlyGuard (ЛЦТ-2026 Кейс 05).
 Связывает входные данные 3D-лидара с ядром FlyGuard и публикует детекции/маркеры.
+
+Топики:
+  вход   <lidar_topic>                sensor_msgs/PointCloud2 (Hesai Pandar128E3X)
+  выход  flyguard/threat_level         std_msgs/String  CLEAR | WARNING | EMERGENCY
+         flyguard/status               std_msgs/String  JSON: дальность, TTC, скорость, рамки
+         flyguard/bounding_boxes       vision_msgs/Detection3DArray
+         flyguard/markers              visualization_msgs/MarkerArray
 """
 
+import json
+import math
 import sys
+import time
 from pathlib import Path
-import numpy as np
 
-# Автоматически добавляем корень репозитория в sys.path
 ROOT_DIR = Path(__file__).resolve().parent.parent
 if str(ROOT_DIR) not in sys.path:
     sys.path.insert(0, str(ROOT_DIR))
@@ -15,189 +23,176 @@ if str(ROOT_DIR) not in sys.path:
 import rclpy
 from rclpy.node import Node
 
-# Импорт сообщений ROS 2
+from geometry_msgs.msg import Point
 from sensor_msgs.msg import PointCloud2
 from std_msgs.msg import String
-from vision_msgs.msg import Detection3DArray, Detection3D, BoundingBox3D
-from visualization_msgs.msg import MarkerArray, Marker
-from geometry_msgs.msg import Vector3
+from vision_msgs.msg import BoundingBox3D, Detection3D, Detection3DArray, ObjectHypothesisWithPose
+from visualization_msgs.msg import Marker, MarkerArray
 
-import sensor_msgs_py.point_cloud2 as pc2
+from flyguard.cdr import from_ros_message
+from flyguard.export import ThreatLevel, export_frame
+from flyguard.mbon_readout import MbonReadout
+from flyguard.mushroom_body import MushroomBody
+from flyguard.pipeline import FlyGuard, Params
+from flyguard.track_readout import TrackReadout
 
-# Импорт ядра FlyGuard
-try:
-    from flyguard.pipeline import FlyGuard, Params
-    from flyguard.mushroom_body import MushroomBody
-except ImportError as e:
-    FlyGuard = None
-    Params = None
-    MushroomBody = None
+ARTIFACTS = ROOT_DIR / "artifacts"
+
+
+def _artifact(param: str, default_name: str) -> Path | None:
+    """Пустой параметр — артефакт по умолчанию из artifacts/, если он есть; '-' — без него."""
+    if param == "-":
+        return None
+    path = Path(param) if param else ARTIFACTS / default_name
+    return path if path.exists() else None
 
 
 class FlyGuardNode(Node):
     def __init__(self):
         super().__init__('flyguard_node')
 
-        # --- Объявление параметров ---
-        self.declare_parameter('lidar_topic', '/pandar_points')
-        self.declare_parameter('frame_id', 'pandar')
+        self.declare_parameter('lidar_topic', '/lidar_points')
+        self.declare_parameter('frame_id', '')
         self.declare_parameter('memory_path', '')
+        self.declare_parameter('readout_path', '')
+        self.declare_parameter('track_readout_path', '-')
         self.declare_parameter('fov_deg', 30.0)
+        # на GPU ускоряется только ламина (EXPERIMENTS п. 7.4); 'auto' — если нужна видеокарта
+        self.declare_parameter('device', 'cpu')
 
-        self.lidar_topic = self.get_parameter('lidar_topic').get_parameter_value().string_value
-        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
-        memory_path = self.get_parameter('memory_path').get_parameter_value().string_value
-        fov_deg = self.get_parameter('fov_deg').get_parameter_value().double_value
+        gp = self.get_parameter
+        self.lidar_topic = gp('lidar_topic').value
+        self.frame_id = gp('frame_id').value
+        memory_path = _artifact(gp('memory_path').value, 'mushroom_body.npz')
+        readout_path = _artifact(gp('readout_path').value, 'mbon_readout.npz')
+        track_path = _artifact(gp('track_readout_path').value, 'track_readout.npz')
 
-        # --- Инициализация ядра FlyGuard ---
-        if FlyGuard is not None:
-            self.get_logger().info("Инициализация ядра FlyGuard...")
-            memory = MushroomBody.load(memory_path) if memory_path else None
-            params = Params(fov_deg=fov_deg)
-            self.fg = FlyGuard(params, memory=memory)
-        else:
-            self.get_logger().error("Модуль flyguard.pipeline не найден!")
-            self.fg = None
+        log = self.get_logger()
+        memory = MushroomBody.load(str(memory_path)) if memory_path else None
+        readout = MbonReadout.load(str(readout_path)) if readout_path else None
+        track_readout = TrackReadout.load(str(track_path)) if track_path else None
+        log.info(f"память тоннеля: {memory_path or 'нет'}; считывание MBON: "
+                 f"{readout_path or 'нет'}; считывание треков: {track_path or 'нет'}")
+        if memory is None or readout is None:
+            log.warn("без памяти тоннеля и считывания MBON ложных тревог будет заметно больше")
+
+        params = Params(fov_deg=float(gp('fov_deg').value), device=gp('device').value)
+        self.fg = FlyGuard(params, memory=memory, readout=readout, track_readout=track_readout)
 
-        # --- Подписчики и Публикаторы ---
         self.sub_cloud = self.create_subscription(
-            PointCloud2,
-            self.lidar_topic,
-            self.pointcloud_callback,
-            10
-        )
-
+            PointCloud2, self.lidar_topic, self.pointcloud_callback, 10)
         self.pub_threat = self.create_publisher(String, 'flyguard/threat_level', 10)
+        self.pub_status = self.create_publisher(String, 'flyguard/status', 10)
         self.pub_boxes = self.create_publisher(Detection3DArray, 'flyguard/bounding_boxes', 10)
         self.pub_markers = self.create_publisher(MarkerArray, 'flyguard/markers', 10)
 
-        self.get_logger().info(f"FlyGuard ROS 2 Node запущен! Топик: {self.lidar_topic}")
+        self.n_frames = 0
+        self.n_failed = 0
+        log.info(f"FlyGuard ROS 2 узел запущен, вход: {self.lidar_topic}, "
+                 f"устройство: {self.fg.device}")
 
     def pointcloud_callback(self, msg: PointCloud2):
-        """Обработка каждого кадра лидара (10 Гц)"""
-        start_time = self.get_clock().now()
-
-        # 1. Читаем точки из PointCloud2 в structured numpy array
-        # (сохраняет стандартную структуру PointCloud)
-        pc_data = pc2.read_points(msg, skip_nans=True)
-        if pc_data.size == 0:
+        t0 = time.perf_counter()
+        # Битый кадр не должен останавливать узел: следующий придёт через 100 мс.
+        try:
+            res = self.fg.process(from_ros_message(msg))
+        except Exception as e:  # noqa: BLE001
+            self.n_failed += 1
+            self.get_logger().error(f"кадр пропущен ({self.n_failed} всего): "
+                                    f"{type(e).__name__}: {e}",
+                                    throttle_duration_sec=1.0)
             return
-
-        # 2. Вызываем ядро FlyGuard
-        if self.fg is None:
+        if res is None:  # идёт калибровка решётки по первым кадрам
             return
+        self.n_frames += 1
 
-        res = self.fg.process(pc_data)
-        if res is None:
-            return
+        frame_id = self.frame_id or msg.header.frame_id
+        out = export_frame(res.decision, self.fg.plane, self.fg.corridor, stamp=res.stamp)
 
-        d = res.decision
-        tracks = getattr(self.fg.cx, 'tracks', [])
+        self.pub_threat.publish(String(data=out.threat_level.name))
+        self.pub_status.publish(String(data=json.dumps(_finite(out.to_dict()),
+                                                       ensure_ascii=False)))
+        self.publish_detections(out, msg.header.stamp, frame_id)
+        self.publish_markers(out, msg.header.stamp, frame_id)
 
-        # 3. Определяем уровень угрозы
-        if d.detected:
-            threat_level = "EMERGENCY" if getattr(d, 'emergency', False) else "WARNING"
-        else:
-            threat_level = "CLEAR"
+        elapsed_ms = (time.perf_counter() - t0) * 1e3
+        if out.threat_level != ThreatLevel.CLEAR:
+            self.get_logger().info(f"{out.threat_level.name}: {out.nearest_distance:.1f} м, "
+                                   f"TTC {out.ttc:.1f} с, кадр {elapsed_ms:.1f} мс",
+                                   throttle_duration_sec=0.5)
+        if elapsed_ms > 100.0:
+            self.get_logger().warn(f"кадр {elapsed_ms:.0f} мс — дольше периода лидара",
+                                   throttle_duration_sec=5.0)
 
-        # 4. Публикация статуса
-        threat_msg = String()
-        threat_msg.data = threat_level
-        self.pub_threat.publish(threat_msg)
-
-        # 5. Публикация маркеров и рамок
-        stamp = msg.header.stamp
-        self.publish_detections(tracks, stamp)
-        self.publish_rviz_markers(tracks, threat_level, d, stamp)
-
-        elapsed_ms = (self.get_clock().now() - start_time).nanoseconds / 1e6
-        self.get_logger().debug(f"Кадр за {elapsed_ms:.1f} мс | Статус: {threat_level} | Треков: {len(tracks)}")
-
-    def publish_detections(self, tracks, stamp):
-        """Формирование vision_msgs/Detection3DArray"""
-        det_array = Detection3DArray()
-        det_array.header.stamp = stamp
-        det_array.header.frame_id = self.frame_id
-
-        for trk in tracks:
+    def publish_detections(self, out, stamp, frame_id):
+        arr = Detection3DArray()
+        arr.header.stamp = stamp
+        arr.header.frame_id = frame_id
+        for b in out.boxes:
             det = Detection3D()
-            det.header = det_array.header
-
+            det.header = arr.header
             bbox = BoundingBox3D()
-            bbox.center.position.x = float(getattr(trk, 'x', 0.0))
-            bbox.center.position.y = float(getattr(trk, 'y', 0.0))
-            bbox.center.position.z = float(getattr(trk, 'z', 0.0))
-
-            bbox.size.x = float(getattr(trk, 'dx', 0.5))
-            bbox.size.y = float(getattr(trk, 'dy', 0.5))
-            bbox.size.z = float(getattr(trk, 'dz', 1.5))
-
+            bbox.center.position.x = b.x
+            bbox.center.position.y = b.y
+            bbox.center.position.z = b.z
+            bbox.center.orientation.z = math.sin(b.yaw * 0.5)
+            bbox.center.orientation.w = math.cos(b.yaw * 0.5)
+            bbox.size.x, bbox.size.y, bbox.size.z = b.dx, b.dy, b.dz
             det.bbox = bbox
-            det_array.detections.append(det)
+            hyp = ObjectHypothesisWithPose()
+            if hasattr(hyp, 'hypothesis'):  # Humble и новее
+                hyp.hypothesis.class_id = b.threat_level.name
+                hyp.hypothesis.score = b.confidence
+            else:  # Foxy
+                hyp.id = b.threat_level.name
+                hyp.score = b.confidence
+            hyp.pose.pose = bbox.center
+            det.results.append(hyp)
+            det.id = str(b.track_id)
+            arr.detections.append(det)
+        self.pub_boxes.publish(arr)
 
-        self.pub_boxes.publish(det_array)
+    def publish_markers(self, out, stamp, frame_id):
+        arr = MarkerArray()
+        clear = Marker()
+        clear.header.frame_id = frame_id
+        clear.action = Marker.DELETEALL
+        arr.markers.append(clear)
+        for m in out.to_rviz_markers(frame_id):
+            arr.markers.append(_marker_from_dict(m, stamp))
+        self.pub_markers.publish(arr)
 
-    def publish_rviz_markers(self, tracks, threat_level: str, decision, stamp):
-        """Генерация маркеров для RViz2"""
-        marker_array = MarkerArray()
 
-        delete_all = Marker()
-        delete_all.action = Marker.DELETEALL
-        marker_array.markers.append(delete_all)
+def _marker_from_dict(d: dict, stamp) -> Marker:
+    m = Marker()
+    m.header.stamp = stamp
+    m.header.frame_id = d["header"]["frame_id"]
+    m.ns, m.id, m.type, m.action = d["ns"], d["id"], d["type"], d["action"]
+    if "pose" in d:
+        p, q = d["pose"]["position"], d["pose"]["orientation"]
+        m.pose.position.x, m.pose.position.y, m.pose.position.z = p["x"], p["y"], p["z"]
+        m.pose.orientation.x, m.pose.orientation.y = q["x"], q["y"]
+        m.pose.orientation.z, m.pose.orientation.w = q["z"], q["w"]
+    else:
+        m.pose.orientation.w = 1.0
+    s = d["scale"]
+    m.scale.x, m.scale.y, m.scale.z = s.get("x", 0.0), s.get("y", 0.0), s.get("z", 0.0)
+    c = d["color"]
+    m.color.r, m.color.g, m.color.b, m.color.a = c["r"], c["g"], c["b"], c["a"]
+    m.points = [Point(x=p["x"], y=p["y"], z=p["z"]) for p in d.get("points", [])]
+    m.text = d.get("text", "")
+    return m
 
-        for i, trk in enumerate(tracks):
-            # Куб вокруг объекта
-            box_marker = Marker()
-            box_marker.header.stamp = stamp
-            box_marker.header.frame_id = self.frame_id
-            box_marker.ns = "flyguard_boxes"
-            box_marker.id = i
-            box_marker.type = Marker.CUBE
-            box_marker.action = Marker.ADD
 
-            x = float(getattr(trk, 'x', 0.0))
-            y = float(getattr(trk, 'y', 0.0))
-            z = float(getattr(trk, 'z', 0.0))
-
-            box_marker.pose.position.x = x
-            box_marker.pose.position.y = y
-            box_marker.pose.position.z = z
-
-            dx = float(getattr(trk, 'dx', 0.5))
-            dy = float(getattr(trk, 'dy', 0.5))
-            dz = float(getattr(trk, 'dz', 1.5))
-            box_marker.scale = Vector3(x=dx, y=dy, z=dz)
-
-            if threat_level == "EMERGENCY":
-                box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 1.0, 0.0, 0.0, 0.6
-            elif threat_level == "WARNING":
-                box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 1.0, 0.9, 0.0, 0.5
-            else:
-                box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 0.0, 1.0, 0.0, 0.4
-
-            marker_array.markers.append(box_marker)
-
-            # Текст с дистанцией над объектом
-            text_marker = Marker()
-            text_marker.header.stamp = stamp
-            text_marker.header.frame_id = self.frame_id
-            text_marker.ns = "flyguard_labels"
-            text_marker.id = 1000 + i
-            text_marker.type = Marker.TEXT_VIEW_FACING
-            text_marker.action = Marker.ADD
-
-            text_marker.pose.position.x = x
-            text_marker.pose.position.y = y
-            text_marker.pose.position.z = z + dz / 2.0 + 0.3
-            text_marker.scale.z = 0.4
-
-            dist = getattr(trk, 'distance', np.hypot(x, y))
-            text_marker.text = f"D: {dist:.1f}m"
-
-            text_marker.color.r, text_marker.color.g, text_marker.color.b, text_marker.color.a = 1.0, 1.0, 1.0, 1.0
-            marker_array.markers.append(text_marker)
-
-        self.pub_markers.publish(marker_array)
+def _finite(v):
+    """JSON без Infinity/NaN: бесконечный TTC у свободного пути уходит как null."""
+    if isinstance(v, float):
+        return v if math.isfinite(v) else None
+    if isinstance(v, dict):
+        return {k: _finite(x) for k, x in v.items()}
+    if isinstance(v, (list, tuple)):
+        return [_finite(x) for x in v]
+    return v
 
 
 def main(args=None):
@@ -209,8 +204,8 @@ def main(args=None):
         pass
     finally:
         node.destroy_node()
-        rclpy.shutdown()
+        rclpy.try_shutdown()
 
 
 if __name__ == '__main__':
-    main()
+    main()