diff --git a/.gitignore b/.gitignore
index 4d41e40..2c25a8d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,11 +2,21 @@ __pycache__/
*.py[cod]
.venv/
.pytest_cache/
-data/
+# записи лидара — только в корне: flyguard/data и .../flyguard/data нужны пакету
+/data/
artifacts/mbon_folds/
+artifacts/track_folds/
docs/figures/*.png
+!docs/figures/curve_gauge.png
+!docs/figures/doubleT_mount_shadow.png
docs/figures/*.mp4
+# сборка ROS 2 и колёса PyTorch для сборки образа без сети
+ros2_ws/build/
+ros2_ws/install/
+ros2_ws/log/
+docker/wheels/*.whl
+
# Project memory & chat transfer
PROJECT_MEMORY.md
CHAT_MEMORY.md
diff --git a/README.md b/README.md
index 5b45061..04445d8 100644
--- a/README.md
+++ b/README.md
@@ -1,241 +1,656 @@
-# FlyGuard · ML-ядро
+# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
-Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара
-Hesai Pandar128. Кейс 05, ЛЦТ-2026.
+Решение кейса «Система обнаружения посторонних объектов для беспилотных поездов в тоннеле метро»
+(хакатон «Лидеры цифровой трансформации 2026», направление «Город»; заказчик — Департамент
+транспорта Москвы / ГУП «Московский метрополитен»).
-Это **ядро обработки**: облако точек на входе, решение о препятствии на выходе.
-Узел ROS 2, транспорт, контейнер и визуализация живут отдельно и сюда не
-входят — ядро от них не зависит и проверяется без ROS вообще.
+На вход — поток облаков точек Hesai Pandar128. На выход — ответ на единственный важный вопрос:
+**«путь свободен» или «впереди препятствие на N метров»**.
---
-## Чем это не является
+## В двух словах
-Не нейросетевой детектор общего назначения. Конвейер собран по схемам
-зрительной системы дрозофилы, и каждая стадия — это конкретный нейропиль с
-конкретной функцией, а не слой, подобранный перебором:
+ТЗ формулирует главный вызов так:
-```
-облако точек 0.3–0.9 млн, 10 Гц
- │
- ├─ RETINA омматидиальная решётка → дальностный образ 128 × N
- ├─ HALTERES плоскость рельсов: крен, тангаж, высота сенсора
- ├─ LAMINA диспаритет 1/R → ON/OFF, центр-окружение на 3 масштабах
- ├─ MEDULLA / LP T4/T5 → LPTC: скорость поезда без одометрии
- ├─ LOBULA LC11: кандидаты; разрез компоненты по контрасту
- ├─ MUSHROOM BODY KC → APL → MBON: новизна формы (без меток)
- │ + обученное считывание MBON (с метками из физики)
- ├─ FAN-SHAPED BODY накопление лучей в координатах пути
- ├─ CENTRAL COMPLEX накопление улик в координатах пути, треки
- └─ DESCENDING два порога с гистерезисом → решение
-```
+> «Возможно, нужно научиться хорошо описывать нормальный тоннель, а затем искать всё, что в него
+> не вписывается.»
-Ничего про геометрию сенсора не захардкожено: решётка лучей, высота установки,
-крен и тангаж **калибруются по самим данным** на первых кадрах. В записях
-встречаются две раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две
-высоты установки (1.31 и 1.70 м) — ядро работает с обеими без правок.
+Это дословное описание того, чем занимается мозг **Drosophila melanogaster**. Муха решает ровно
+нашу задачу — на лету, без разметки, без одометрии, с мизерным бюджетом нейронов и в жёстком
+реальном времени. Поэтому архитектура FlyGuard собрана из её вычислительных схем, взятых из
+коннектома (FlyWire / hemibrain):
----
-
-## Что нужно интеграции: один класс, один вызов
-
-```python
-from flyguard.pipeline import FlyGuard, Params
-from flyguard.mushroom_body import MushroomBody
-from flyguard.mbon_readout import MbonReadout
-
-fg = FlyGuard(Params(),
- memory=MushroomBody.load("artifacts/mushroom_body.npz"),
- readout=MbonReadout.load("artifacts/mbon_readout.npz"))
-
-res = fg.process(cloud) # cloud: flyguard.cdr.PointCloud2
-if res is None:
- ... # первые 12 кадров уходят на калибровку решётки
-else:
- d = res.decision # detected, distance, confidence, emergency, objects
- res.total_ms # время обработки кадра
-```
-
-Требования к входу: `flyguard.cdr.PointCloud2` — поля `x, y, z, intensity`,
-порядок точек как в сыром CDR. Конвейер **хранит состояние между кадрами**
-(решётка, плоскость пути, ось пути, треки, накопитель), поэтому один экземпляр
-обслуживает один поток данных; для параллельных сценариев нужны разные
-экземпляры.
-
-Выход `Decision`: `detected`, `distance` (м), `confidence` (0…1),
-`emergency` (флаг экстренного торможения), `objects` (список подтверждённых
-треков с id, дистанцией и габаритами).
-
-Всё считается на **CPU**, GPU не требуется. Медиана обработки кадра — 43 мс
-при бюджете 100 мс (замер по стадиям, один процесс, EXPERIMENTS п. 7.4).
-
-Ламину можно перенести на **NVIDIA GPU** (`Params(device="auto")` или
-`"cuda"`, модуль `flyguard/device.py` с переходом на CPU при любом сбое карты).
-Замерено на RTX 5070 Ti: ламина 5.8 → 2.9 мс с копированием туда и обратно,
-то есть кадр 43 → ~40 мс, — остальные 80 % времени кластеризация и геометрия,
-которые видеокарта не ускоряет. Поэтому по умолчанию `device="cpu"`: выигрыш
-в 3 мс не стоит отдельного образа и `--gpus all` на машине проверки.
-
----
-
-## Структура
-
-```
-flyguard/ ядро: стадии обработки, память, считывание
- bag.py cdr.py чтение rosbag2 и разбор CDR без ROS
- device.py выбор CPU / NVIDIA GPU и переход на CPU при сбое
- retina.py geometry.py решётка лучей, плоскость рельсов, ось пути
- lamina.py medulla.py контраст (на CPU или GPU), движение
- lobula.py кандидаты
- mushroom_body.py память тоннеля (без меток)
- mbon_readout.py обученное считывание (с метками)
- track_readout.py считывание по истории трека — инструмент замера
- fan_body.py накопление в координатах пути
- central_complex.py треки и улики
- descending.py решение
- pipeline.py сборка
- export.py 3D-рамки, время до столкновения, маркеры RViz
- synth.py вставка предметов трассировкой лучей
-tools/ обучение, оценка, разбор, полигон с аугментациями
-tests/ 53 теста, запускаются без данных и без ROS
-docs/ методика и результаты
-artifacts/ обученные модели
-```
-
----
-
-## Как запустить
-
-```bash
-pip install -r requirements.txt
-pytest tests -q # или без pytest: python tests/run_tests.py
-```
-
-Через Docker (подробно — [docs/DOCKER.md](docs/DOCKER.md)):
-
-```bash
-./docker-run.sh build
-docker compose run --rm test # тесты
-docker compose run --rm info # что видно из контейнера: CPU, GPU, CUDA
-docker compose run --rm evaluate # ложные тревоги
-docker compose run --rm benchmark # полигон
-```
-
-Для обучения на видеокарте — `pip install -r requirements-gpu.txt` и
-`--device cuda` у `train_mbon.py`, `train_mushroom_body.py`, `evaluate.py`.
-
-Записи лидара в репозиторий не кладутся. Положите их рядом
-(`../data/for_hackathon/...`) или укажите путь:
-
-```bash
-set FLYGUARD_DATA=D:\lidar\data
-```
-
-Обучение и оценка:
-
-```bash
-python tools/make_training_set.py # размеченная выборка
-python tools/train_mbon.py --device cuda --baseline # считывание MBON
-python tools/evaluate.py --mbon-dir artifacts/mbon_folds # ложные тревоги
-python tools/make_benchmark.py --memory artifacts/mushroom_body.npz \
- --mbon-dir artifacts/mbon_folds # дальность обнаружения
-python tools/plot_benchmark.py # кривые и график
-python tools/compare_benchmark.py было.json стало.json # правка парно
-```
-
-Большой бэг `new_data` (90 ГБ) не распаковывается целиком — инструменты
-читают его кусками прямо из архива (`--tar`, по умолчанию
-`../датасет/new_data` или `FLYGUARD_NEW_DATA`):
-
-```bash
-python tools/make_training_set.py --new-data 0:110 --out data/cache/training_set_nd.npz
-python tools/train_mbon.py --device cuda --data data/cache/training_set.npz \
- --data data/cache/training_set_nd.npz --train-only new_data_ --out nd.npz
-python tools/eval_new_data.py --shards 110: \
- --readout было=artifacts/mbon_readout.npz --readout стало=nd.npz --out nd.json
-```
-
-Проверка на второй половине честная: вставки идут только в первую, а
-вторую модель не видит ни в каком виде (EXPERIMENTS п. 17.3).
-
-Синтетику организаторов (`cloud_with_fake_obj`, 10 предметов) сверяет с
-эталоном отдельный инструмент. Разметки к бэгу нет, но вставленные точки не
-лежат на элевациях колец, и эталон извлекается из самого бэга (п. 18.2):
-
-```bash
-python tools/eval_org_synth.py # итоговые настройки
-python tools/eval_org_synth.py --set half_width=1.6 --set h_top=0
-```
-
-В этом бэге нет поля `ring`, а порядок точек сбит вставками: `retina.py`
-восстанавливает кольца по элевации и раскладывает такие кадры по углам точек.
-До этой правки ядро на нём не обрабатывало ни одного кадра (п. 18.1).
-
-Любое поле `Params` меняется без правки кода: `--set h_lo_core=0.28 --set
-k_sigma=0.75` у `make_benchmark.py`, `evaluate.py`, `make_training_set.py` и
-`check_obstacle.py`. Два прогона полигона сравнивайте только парно —
-`compare_benchmark.py` считает, сколько наблюдений перевернулось в каждую
-сторону на одних и тех же вставках. Итоговые таблицы двух прогонов шумят
-сильнее, чем меняет их большинство правок (EXPERIMENTS п. 16.1).
-
-Тяжёлые шаги сами раскладываются по бэгам на процессы — записей пять, физических
-ядер шесть, и это вся доступная зернистость: конвейер держит состояние между
-кадрами, поэтому разрезать одну запись нельзя. Замерено: полигон 134 → 36 с,
-сбор выборки 96 → 26 с на облегчённой конфигурации, то есть 3.7–3.8×, и файл на
-выходе совпадает с последовательным **побайтово**. Отключается `--jobs 1`.
-
-Для замера задержки кадра `--jobs 1` обязателен: под пятью процессами время
-кадра растёт с 32 до 56 мс. Это свойство замера, а не конвейера, поэтому
-`evaluate.py` в параллельном режиме печатает задержку как `nan` — чтобы такое
-число нельзя было случайно привести в отчёте.
-
----
-
-## Где мы сейчас
-
-| Метрика | Значение | Чем измерено |
+| Проблема кейса | Схема мухи | Модуль |
|---|---|---|
-| Реальный объект 0.67 × 1.35 м на 55 м | **99.5 %** кадров | `tools/check_obstacle.py` |
-| Синтетика организаторов, 10 предметов | **9 из 10** верно | `tools/eval_org_synth.py` |
-| Ложные тревоги, leave-one-bag-out | **5.7 трека на км**, 8.4 % кадров | `tools/evaluate.py --mbon-dir` |
-| То же без обученного считывания | 11.9 на км, 20.3 % кадров | там же, без `--mbon-dir` |
-| На незнакомой линии (памяти нет) | **12.8 на км**; без считывания 36.1 | `tools/evaluate.py --no-memory` |
-| Вторая половина `new_data`: другой день, 3.41 км, не видена при обучении | **11.7 на км**, 17.9 % кадров | `tools/eval_new_data.py` |
-| Дальность (полигон, 15 560 наблюдений) | человек стоя: рабочая дальность **100 м**, P@50 = 0.72, P@100 = 0.52, P@150 = 0.46 | `tools/plot_benchmark.py` |
-| Человек, упавший на пути | P@50 = **0.50** (было 0.27), рабочая дальность 20 м | там же |
-| Обработка кадра | медиана 33 мс из бюджета 100 мс; на ядре уровня стенда жюри 55 / 65 мс (медиана / p95) | контейнер, EXPERIMENTS п. 18.5, 19.5 |
-| Разделение «знакомое / новое» (без учителя) | ROC AUC 0.905 | `tools/tune_memory.py` |
-| Считывание MBON «предмет / тоннель» (с учителем) | ROC AUC **0.986** | `tools/train_mbon.py` |
+| Разметки нет, почти всё — пустой тоннель | грибовидное тело: KC + APL + MBON, подавление **знакомого** | `mushroom_body.py` |
+| Нельзя путать своё движение с чужим объектом | T4/T5 → LPTC, широкопольный оптический поток | `medulla.py` |
+| Увидеть заранее, независимо от размера | LPLC2 — детектор надвигания | `medulla.py` |
+| Мелкий объект = 5 лучей на кадр | кольцевой аттрактор эллипсоидного тела: накопление улик | `central_complex.py` |
+| На 170 м контраст к фону равен нулю | веерное тело: улики копятся в координатах мира, а не кадра | `fan_body.py` |
+| Не реагировать на штатные конструкции | депрессия синапсов KC→MBON | `mushroom_body.py` |
+| Сенсор закреплён нежёстко | жужжальца и оцеллии: стабилизация «взгляда» | `geometry.py` |
+| 100 мс на кадр, слабый CPU | разрежённый бинарный код, 0.1 % активных клеток | `mushroom_body.py` |
-Проверка всегда **leave-one-bag-out**: память обучается на всех записях, кроме
-проверяемой. Иначе цифры лгут — подавлять конструкции, которые сам же и
-запомнил, умеет кто угодно, а на приватном тесте будет новый участок.
+Подробный разбор с привязкой к типам нейронов — в [docs/ALGORITHM.md](docs/ALGORITHM.md)
+и [docs/CONNECTOME.md](docs/CONNECTOME.md).
---
-## Что честно не работает
+## Архитектура
-Разобрано замерами, подробности — в `docs/EXPERIMENTS.md`:
+```
+ROS 2 bag → /lidar_points (PointCloud2, 0.3–0.9 млн точек, 10 Гц)
+ │
+ ├─ RETINA омматидиальная решётка → дальностный образ 128 × N
+ ├─ HALTERES плоскость пути: крен, тангаж, высота сенсора
+ ├─ LAMINA диспаритет 1/R, ON/OFF, центр-окружение на 3 масштабах
+ ├─ MEDULLA/LP T4/T5 → LPTC: скорость поезда без одометрии; LPLC2: надвигание
+ ├─ LOBULA LC11: кандидаты; разрез по контрасту отделяет предмет от стены
+ ├─ MUSHROOM BODY новизна: подавление знакомой обстановки тоннеля
+ ├─ FAN-SHAPED BODY накопление лучей в координатах пути: улика там, где нет контраста
+ ├─ CENTRAL COMPLEX накопление улик в координатах пути, треки
+ └─ DESCENDING два порога: предупреждение и экстренное торможение
+ │
+ ▼
+/flyguard/obstacle · /flyguard/markers · /flyguard/brain · /flyguard/diagnostics
+```
-* **За 200 м на этих участках не увидит никто**: прямая видимость в тоннелях
- 121–167 м, дальше линия взгляда упирается в стену кривой. На отдельных
- перегонах и того меньше — 49–90 м.
-* **Мелкие предметы на большой дальности невозможны с этим сенсором**: ведро
- (0.1 м²) на 160–190 м даёт один луч, каска и бутылка — ноль.
-* **За 80–90 м прирельсовая зона не наблюдается вовсе**: луч скользит по
- полотну, и самая низкая видимая точка у оси пути оказывается выше головки
- рельса на 0.1–0.6 м.
-* Привыкание внутри проезда сделано и **отвергнуто замером** — п. 10.
-* **Яркость как признак мы себе не засчитываем.** Абсолютной шкалы
- интенсивности в записях нет: медиана по кандидатам обстановки 3…7 в пяти
- бэгах и 23.5 в шестом. Вставка берёт яркость реальных возвратов с тех же
- лучей, то есть признак намеренно обесточен, и дальность из-за этого
- занижена — настоящий предмет был в 1.47 раза ярче окружения. Разбор — п. 11.1.
+Детально — [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md).
---
-## Документация
+## Сборка
-* `docs/ALGORITHM.md` — что делает каждая стадия и почему именно так.
-* `docs/EXPERIMENTS.md` — все замеры, включая отрицательные результаты.
-* `docs/CONNECTOME.md` — что взято из коннектома как число, а что как идея.
+```bash
+docker build -t flyguard -f docker/Dockerfile .
+```
+
+Образ основан на `ros:humble-ros-base-jammy` (Ubuntu 22.04 + ROS 2 Humble). Все зависимости
+ставятся на этапе сборки; **во время работы сеть не нужна**. Обе обученные части лежат в
+образе и подключаются сами: память тоннеля (`artifacts/mushroom_body.npz`) и считывание MBON
+(`artifacts/mbon_readout.npz`) — ровно та конфигурация, что замерена в разделе «Результаты».
+
+Собранный образ проверяется одной командой:
+
+```bash
+docker run --rm -v "$PWD/docker:/smoke:ro" flyguard bash /smoke/smoke_test.sh
+```
+
+### Стенд без интернета
+
+Организаторы подтвердили: на тестовом сервере сети нет. Узлу она и не нужна — всё
+ставится при сборке. Но сама `docker build` без сети не пройдёт: ей нужны базовый образ
+`ros:humble-ros-base-jammy` и пакеты apt. Поэтому образ собирается там, где сеть есть, и
+переносится файлом:
+
+```bash
+docker save flyguard | gzip > flyguard_image.tar.gz # на машине с сетью
+```
+```bash
+docker load -i flyguard_image.tar.gz # на стенде
+```
+
+После `docker load` все команды ниже работают как есть. Образ — 9.3 ГБ, из них 6.6 ГБ —
+PyTorch с библиотеками CUDA (раздел «Видеокарта»); архив — около 5 ГБ. Наш готовый архив —
+`flyguard_image.tar`, 4.7 ГБ: слои в нём уже сжаты, поэтому `gzip` поверх не нужен, а
+`docker load -i` принимает и `.tar`, и `.tar.gz`. Проверено на чистом движке: архив загружен в
+Docker Desktop 29.8 (Windows 11, WSL 2) и работает с `--gpus all` и без него.
+
+Если сети нет и там, где собирается образ, колёса PyTorch скачиваются на соседней машине
+(`python docker/fetch_wheels.py`) и отдаются сборке любым HTTP-сервером — аргумент
+`TORCH_WHEELS`, подробности в [docker/wheels/README.md](docker/wheels/README.md).
+
+## Запуск
+
+Одной командой — детектор и проигрывание бэга вместе:
+
+```bash
+docker run --rm -it --gpus all --network host --ipc host -v /path/to/bags:/data flyguard \
+ ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle
+```
+
+Или раздельно — узел в контейнере, а `ros2 bag play` на хосте, как на поезде:
+
+```bash
+docker run --rm -it --gpus all --network host --ipc host flyguard
+```
+
+```bash
+ros2 bag play --read-ahead-queue-size 10 /path/to/bags/doubleT_obstacle
+```
+
+**`--ipc host` обязателен, если bag проигрывается на хосте.** С `--network host` Fast DDS
+считает контейнер и хост одной машиной и передаёт кадры через разделяемую память
+`/dev/shm`, а без `--ipc host` у контейнера она своя. Кадры тогда теряются молча: замерено,
+узел не получает **ни одного** кадра, а с флагом — 252 из 252. Если флаг всё же забыт, узел
+через пять секунд после появления издателя пишет в журнал, в чём дело. Проигрывание внутри
+контейнера (первая команда или второй контейнер из этого же образа) работает и без флага:
+точка входа видит собственную `/dev/shm` и переводит транспорт на UDP.
+
+Профиль транспорта для кадров в 24 МБ лежит в образе и подключается сам
+(`docker/fastdds_large.xml`); снять — `FLYGUARD_DDS_PROFILE=0`.
+
+`--read-ahead-queue-size 10` тоже не косметика: по умолчанию проигрыватель читает вперёд
+1000 сообщений, при кадре в 24 МБ это 24 ГБ, и пока он их читает, первые секунды записи
+успевают «просрочиться» и не публикуются вовсе (без флага доходит 119 кадров из 201).
+В первой команде launch ставит его сам.
+
+С RViz окну нужен доступ к экрану хоста:
+
+```bash
+xhost +local:
+docker run --rm -it --network host --ipc host -e DISPLAY=$DISPLAY \
+ -v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
+ ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true
+```
+
+**RViz в контейнере без доступа к видеокарте рисует программно** (Mesa `llvmpipe` на
+процессоре): вращение мышью идёт рывками по 2–5 кадров в секунду, хотя в настройках стоит
+`Frame Rate: 30` — это потолок, а не факт. Замер на `doubleT_obstacle`: программный RViz
+занимает 1.9 ядра, с видеокартой — 0.2 ядра, в 9 раз меньше. Видеокарта пробрасывается так:
+
+| Где | Добавить к `docker run` |
+|---|---|
+| Linux, NVIDIA (нужен nvidia-container-toolkit) | `--gpus all` (графика драйвера включена в образе: `NVIDIA_DRIVER_CAPABILITIES=all`) |
+| Linux, встроенная Intel/AMD | `--device /dev/dri` |
+| Windows: Docker Desktop или Docker в WSL 2 | `--device /dev/dxg -v /usr/lib/wsl:/usr/lib/wsl:ro -e LD_LIBRARY_PATH=/usr/lib/wsl/lib` (этих флагов хватает и для CUDA; одного `--gpus all` в Docker Desktop мало — CUDA будет, а RViz рисует процессором) |
+
+Проверено на Windows 11 + WSL 2 (RTX 5070 Ti, `D3D12`) — в Docker внутри WSL и в Docker
+Desktop 29.8; строки для Linux — стандартные флаги, на стенде не проверялись. Экран на
+Windows даёт WSLg: из терминала Ubuntu — `-e DISPLAY=:0 -v /tmp/.X11-unix:/tmp/.X11-unix`,
+из PowerShell с Docker Desktop — `-e DISPLAY=:0 -v /run/desktop/mnt/host/wslg/.X11-unix:/tmp/.X11-unix`.
+Записи на Windows держите в файловой системе WSL (`/root/bags` и т. п.): с диска `C:` через
+общую папку Docker Desktop кадры по 24 МБ идут около одного в секунду — замерено, за минуту
+дошло 62 кадра из 201. Чем рисует OpenGL, показывает проверка без окна:
+
+```bash
+docker run --rm -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix <флаги видеокарты> \
+ -v "$PWD/docker:/smoke:ro" flyguard python3 /smoke/gl_probe.py
+```
+
+Можно и вовсе запустить RViz на хосте с нашим конфигом, если там стоит ROS 2:
+`rviz2 -d ros2_ws/src/flyguard/config/flyguard.rviz`. На обнаружение всё это не влияет:
+детектор с программным RViz и с аппаратным обработал одни и те же 190 кадров из 190.
+
+RViz показывает не сырое облако, а **облако обзора**, которое публикует сам узел
+(`/flyguard/view_cloud`): лучи сектора обработки, выровненные по плоскости рельсов, — до
+77 тысяч точек вместо 900 тысяч. Рамки препятствий стоят в тех же координатах, пол пути
+лежит на сетке. Сырое облако RViz не тянет (24 МБ на кадр), и топик у записей разный —
+`/lidar_points` в пяти записях и `/sensing/lidar/hesai128/pointcloud` в `doubleT_obstacle`, —
+а облако обзора есть всегда, из какого бы топика узел ни читал. Публикуется, только когда на
+него кто-то подписан, и в замеры задержки не входит.
+
+**Оранжевый контур — габарит, который узел проверяет.** Ось пути впереди узел оценивает в
+каждом кадре по сечению тоннеля, и в кривой габарит изгибается вместе с ней. Проверяется
+объединение изогнутого габарита и прямого, вдоль оси лидара: неточная оценка оси не должна
+сужать зону поиска. На прямом пути контуры совпадают, в кривой рисуются оба — изогнутый ярко,
+прямой бледно ([рисунок](docs/figures/curve_gauge.png)). Рамки препятствий стоят у своих
+точек. На кривой синтетики организаторов (радиус около 760 м) ось сходится с положением
+предмета до 0.04 м на 22 м, до 0.15 м на 39 м и до 0.27 м на 64 м (EXPERIMENTS п. 20).
+
+Чтобы не ждать до нужного места, проигрывание начинается с любой секунды: `start:=18`.
+
+### Видеокарта
+
+Раскладка точек по решётке, ламина и кластеризация — три стадии, работающие с целым
+образом 128 × 600 лучей, — считаются на видеокарте, если узел её видит; остальное на
+процессоре. Для этого контейнеру нужен `--gpus all` (на Linux — с nvidia-container-toolkit).
+Без флага, без видеокарты или при любом её сбое на ходу узел считает на процессоре — **с тем
+же результатом**: на видеокарте те же операции в тех же типах, и решения совпадают с
+процессорными покадрово на синтетике и шести записях ([EXPERIMENTS](docs/EXPERIMENTS.md),
+п. 21). Где идёт счёт, узел пишет в журнал:
+
+```
+вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне (в Docker под WSL до 20 с), результат тот же
+вычисления: видеокарта NVIDIA GeForce RTX 5070 Ti (16 ГБ, CUDA 12.8) — сетчатка, ламина, кластеризация; остальное на процессоре (готова через 16.0 с после старта)
+```
+
+**Видеокарта поднимается в фоне**, а узел подписывается на лидар сразу и первые кадры считает
+на процессоре; на видеокарту он переходит между кадрами. Первый запуск ядер CUDA в новом
+контейнере под WSL длится 15 с (на Windows без контейнера — 0.9 с), и раньше узел всё это
+время не был подписан: запись, пущенная сразу, теряла первые 15 с. Теперь запуск одновременно
+с записью даёт те же 201 кадр из 201 и 189 обнаружений из 190. Зависни драйвер совсем, узел
+останется на процессоре, а не встанет.
+
+Кадр в контейнере (медиана / p95), видеокарта против процессора:
+
+| Где | `doubleT_obstacle` | синтетика организаторов |
+|---|---|---|
+| Docker Desktop 29.8 (Windows 11, WSL 2), `--gpus all` | 23.9 / 35.2 мс против 34.8 / 39.4 | 26.4 / 42.6 мс против 42.7 / 48.0 |
+| Docker Engine в Ubuntu под WSL, `/dev/dxg` | 23.4 / 30.2 мс против 31.8 / 34.9 | — |
+
+Решения одинаковые: 189 обнаружений из 190 и 398 кадров с тревогой из 1499 в обоих режимах.
+Принудительно на процессоре — `device:=cpu`. В Docker Engine, поставленном прямо в Ubuntu под
+WSL (без Docker Desktop), вместо `--gpus all`:
+`--device /dev/dxg -v /usr/lib/wsl:/usr/lib/wsl:ro -e LD_LIBRARY_PATH=/usr/lib/wsl/lib`.
+Под WSL драйвер CUDA пишет в каждый новый контейнер свой кэш, около 330 МБ, а виртуальный диск
+Docker от этого растёт и сам не сжимается; если на диске тесно — `-e CUDA_CACHE_DISABLE=1`, на
+скорость это не влияет.
+
+### Пауза, перемотка, покадрово
+
+Проигрыватель, запущенный из launch (`bag:=...`), клавиатуры не слышит. Для него в образе
+есть пульт: контейнер запускается с именем, а пульт — во втором терминале.
+
+```bash
+docker run --rm -it --name flyguard_demo --network host --ipc host -e DISPLAY=$DISPLAY \
+ -v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
+ ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true
+```
+```bash
+docker exec -it flyguard_demo flyguard-keys /data/doubleT_obstacle
+```
+
+| Клавиша | Действие |
+|---|---|
+| пробел | пауза / продолжить |
+| ← → | на 3 секунды назад / вперёд |
+| `.` | один кадр вперёд (на паузе) |
+| ↑ ↓ | быстрее / медленнее (×0.1 … ×4) |
+| `0` | в начало записи |
+| `q` | выйти, запись играет дальше |
+
+На паузе RViz держит последний кадр и рамки, и сцену можно крутить. После перемотки узел сам
+замечает скачок времени записи и начинает треки, одометрию и ось пути заново — иначе
+предмет с прошлого места всплыл бы там, где его нет; то же самое происходит на каждом круге
+`ros2 bag play --loop`. Если `ros2 bag play` запущен вручную в своём терминале, пульт не нужен:
+у проигрывателя свои клавиши (пробел — пауза, → — следующий кадр, ↑ ↓ — скорость).
+
+Все записи подряд, по строке сводки на каждую (узел поднимается заново для каждой записи):
+
+```bash
+docker run --rm --network host --ipc host -v /path/to/bags:/data:ro \
+ -v "$PWD/docker:/smoke:ro" flyguard bash /smoke/check_all.sh
+```
+
+Итог узел пишет прямо в консоль контейнера — смену состояния и, пока тревога держится,
+ближайшую дальность раз в секунду; когда тревога снимается, пишет `путь свободен`. Так
+выглядит запись с настоящим препятствием (`doubleT_obstacle`, объект на 55–57 м):
+
+```
+[detector-1] [WARN] [1790249076.653486850] [flyguard]: ПРЕПЯТСТВИЕ: 55.7 м, уверенность 0.36, объектов 1
+[detector-1] [WARN] [1790249327.983883704] [flyguard]: препятствие: 55.9 м, уверенность 0.51, объектов 1
+[detector-1] [WARN] [1790249336.988973904] [flyguard]: препятствие: 56.0 м, уверенность 0.68, объектов 2
+```
+
+Если поезд движется, к строке добавляется время до столкновения.
+
+Полный программный выход без RViz:
+
+```bash
+ros2 topic echo /flyguard/obstacle
+```
+
+### Облака без поля `ring` и с нарушенным порядком точек
+
+Синтетический бэг организаторов (`cloud_with_fake_obj`, 24.09) устроен не так, как записи с
+поезда: в облаке только `x, y, z, intensity`, без `ring` и `timestamp`, а там, где вставлен
+предмет, заслонённые им точки удалены и точки предмета вписаны в середину массива. Число точек
+в кадре гуляет от 214 до 353 тысяч, и порядок «столбец · эхо · кольцо» нарушен даже в кадрах
+ровно на 307 200 точек — ни в одном из первых двенадцати он не цел. Узел это переносит сам:
+
+* кольца восстанавливаются по гистограмме элевации (у лазерного канала она постоянна до
+ 0.0001°, соседние каналы Pandar128 разнесены на 0.086°);
+* каждый кадр проверяется: у целого кадра элевация каждой точки совпадает с элевацией её
+ кольца. Такой кадр идёт быстрым путём по порядку точек, а кадр с нарушенным порядком
+ раскладывается в ту же решётку **по углам каждой точки**. На целом кадре оба пути дают
+ образ бит в бит; раскладка по углам дороже: 11 мс против 6 на синтетике (скан 120°),
+ 22 против 6 на круговом скане. В контейнере кадр синтетики — 43.7 / 50.2 мс (медиана / p95).
+
+До этой правки узел на синтетике не обработал бы ни одного кадра: калибровка решётки
+требовала поле `ring`.
+
+### Демонстрация со схемой мозга мухи
+
+```bash
+docker run --rm -it --network host --ipc host -e DISPLAY=$DISPLAY \
+ -v /tmp/.X11-unix:/tmp/.X11-unix -v /path/to/bags:/data flyguard \
+ ros2 launch flyguard detect.launch.py bag:=/data/doubleT_obstacle rviz:=true brain_view:=true
+```
+
+Топик `/flyguard/brain` отдаёт мозг дрозофилы, подсвеченный **живой активностью**: видно,
+как загорается ламина на контрасте, лобулярная пластинка на скорости, доли грибовидного тела
+на новизне и гигантское волокно в момент решения. Вид отключён по умолчанию, чтобы не попадать
+в замеры задержки; когда включён, рисуется пять раз в секунду по 15 мс. С `brain_view:=true`
+RViz берёт свой конфиг: схема справа во всю высоту, панель Displays свёрнута — открывается
+стрелкой у левого края окна.
+
+Три варианта, `brain_style`:
+
+| Значение | Что показывает |
+|---|---|
+| `scheme` | нарисованная схема нейропилей с подписями |
+| `cloud` | **139 255 настоящих нейронов** коннектома FlyWire на своих анатомических местах |
+| `hybrid` | по умолчанию: сверху панели решётки лучей (дальность, ON/OFF ламины, кандидаты), снизу облако нейронов |
+
+Облако — не симуляция. Мембранные потенциалы 139 тысяч клеток никто не интегрирует: коннектом
+даёт анатомию и принадлежность клеток, конвейер даёт активность по стадиям, а вид накладывает
+одно на другое. Привязка держится на именах типов клеток, и все они есть в выгрузке поимённо —
+LC11 (127 нейронов), LPLC2 (210), HS/VS (22), T4/T5 (12 245), клетки Кеньона (5177),
+MBON (96), APL (2), гигантское волокно DNp01 (2). Ровно те схемы, из которых собран FlyGuard.
+
+Для экрана и видео вид рисуется крупнее: `brain_scale:=2` — 2360 × 1572 (под 2K),
+`brain_scale:=3` — 3540 × 2358 (под 4K). Сомы берутся из атласа своего размера, это настоящие
+координаты, а не растянутая картинка; шрифты, линии и отступы растут вместе с масштабом.
+Кадр вида рисуется 14 мс в 1×, 57 мс в 2× и 113 мс в 3×, пять раз в секунду; в 2× на записи
+с поезда и на синтетике узел принял все кадры. По умолчанию масштаб 1 — прежние 1180 пикселей
+по ширине.
+
+Атласы (по 620–660 КБ, три размера) лежат в пакете и пересобираются из публичных выгрузок Codex:
+
+```bash
+python tools/build_brain_atlas.py
+python tools/build_brain_atlas.py --width 2360 --height 1240 --margin 36 --out ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz
+```
+
+Данные FlyWire — CC-BY 4.0 (Dorkenwald et al., Schlegel et al., Nature 2024).
+
+---
+
+## Выходные данные
+
+| Топик | Тип | Назначение |
+|---|---|---|
+| `/flyguard/obstacle` | `flyguard_msgs/ObstacleStatus` | основной программный выход |
+| `/flyguard/detected` | `std_msgs/Bool` | бинарный статус для простой интеграции |
+| `/flyguard/distance` | `std_msgs/Float32` | расстояние до ближайшего объекта, м (−1 — свободно) |
+| `/flyguard/markers` | `visualization_msgs/MarkerArray` | рамки объектов для RViz |
+| `/flyguard/view_cloud` | `sensor_msgs/PointCloud2` | облако обзора для RViz: сектор обработки в координатах пути, только при подписчике |
+| `/flyguard/brain` | `sensor_msgs/Image` | мозг мухи с живой активностью (схема, облако нейронов или гибрид) |
+| `/flyguard/diagnostics` | `diagnostic_msgs/DiagnosticArray` | задержки по стадиям, скорость, радиус кривой |
+
+`ObstacleStatus` содержит: `detected`, `emergency`, `distance`, `time_to_collision`,
+`confidence`, `speed`, `stopping_distance`, `processing_ms` и список объектов с габаритами,
+числом лучей, новизной и стабильным `track_id`.
+
+---
+
+## Параметры
+
+Все настройки — в [ros2_ws/src/flyguard/config/flyguard.yaml](ros2_ws/src/flyguard/config/flyguard.yaml),
+переопределяются при запуске:
+
+```bash
+ros2 launch flyguard detect.launch.py fov_deg:=35.0 half_width:=1.5
+```
+
+| Параметр | По умолчанию | Смысл |
+|---|---|---|
+| `input_topic` | `/lidar_points` | топик лидара. Узел слушает и запасные имена (`/sensing/lidar/hesai128/pointcloud`, `/points_raw`), а если облако идёт в топик с другим именем, через секунду находит его сам, подписывается и пишет об этом в журнал |
+| `device` | `auto` | где считать сетчатку, ламину и кластеризацию: `auto` — видеокарта, если есть (контейнер с `--gpus all`), иначе процессор; `cuda`; `cpu`. Видеокарта поднимается в фоне, первые кадры считает процессор. Результат одинаков, см. раздел «Видеокарта» |
+| `mbon_path` | из образа | обученное считывание MBON (`.npz`); пусто — ручная формула веса улики |
+| `mbon_power` | `1.5` | резкость считывания: вес наблюдения — вероятность в этой степени, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 17.4 |
+| `half_width` | `1.2` | полуширина габарита, м. Так его задают организаторы в синтетике 24.09: «у края» — до 1.13 м от оси, «вне габарита, но близко» — с 1.14 м. При прежних 1.6 второй давал ложную тревогу, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 18.4 |
+| `h_lo` / `h_hi` | `0.28` / `2.3` | границы основной части габарита по высоте над головкой рельса, м |
+| `h_top` / `half_width_top` / `top_d_max` | `3.3` / `1.0` / `90` | верхняя секция: от `h_hi` до `h_top`, уже основной и не дальше `top_d_max` — то, что свисает со свода в путь вагона. Ищется отдельным проходом (`top_detect`), в котором свод не склеивается со свисающим; стоящее, что уходит вниз к полу, остаётся основному проходу. Верх 3.3, а не выше: у свода свои кабели и кронштейны, и на 60–90 м ошибка наклона опускает их в секцию. `h_top: 0` — выключить |
+| `flat_h` / `flat_top` / `flat_d` / `flat_w` | `0.06` / `0.26` / `70` / `0.15` | плоское у пола: высотой меньше `flat_h`, целиком ниже `flat_top` над рельсом, не дальше `flat_d` — вес наблюдения умножается на `flat_w`. Пластины на полотне, края жёлоба, порог гермозатвора: организаторы подтвердили, что в жёлобе — не препятствие. `flat_h: 0` — выключить |
+| `h_lo_core` / `core_from` | `0.16` / `30.0` | пол между рельсами и дальность, с которой он опущен: иначе упавший на пути человек (0.30 м) виден верхушкой в два сантиметра. Ближе 30 м пол прежний — там в полосу попадают головки рельсов, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 16.3 |
+| `d_min` / `d_max` | `4.0` / `220.0` | зона поиска по дальности, м |
+| `fov_deg` | `30.0` | полусектор обработки по азимуту, ° |
+| `min_rays` | `4` | минимум лучей на кандидата |
+| `memory_path` | из образа | обученная память тоннеля (`.npz`) |
+| `brain_view` | `false` | публиковать вид мозга |
+| `brain_style` | `hybrid` | `scheme` · `cloud` · `hybrid` — что именно рисовать |
+| `brain_scale` | `1` | масштаб вида мозга: `2` — 2360 × 1572 для 2K, `3` — для 4K (облако и гибрид) |
+| `ctx_up` | `4.0` | насколько кластеризация смотрит выше габарита, м; меньше — и колонна, срезанная по верхней границе, выглядит предметом |
+| `split_adv` | `0.0` | разделение фигуры и фона по скорости сближения: рабочая дальность 62 → 80 м ценой вчетверо больших ложных тревог. По умолчанию выключено, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 9.4 |
+| `enable_accumulator` | `true` | накопление лучей в координатах пути: удваивает рабочую дальность там, где локальный контраст равен нулю |
+| `acc_near` / `acc_gain` | `55.0` / `1.5` | с какой дальности включается накопление и какая опора считается полным контрастом |
+| `split_gap` | `6.0` | разрез компоненты, растёкшейся вдоль стены, по контрасту ламины: гладкая стена даёт ноль по построению, предмет на ней — ступеньку. `0` — выключить |
+| `split_near` / `split_top` | `55.0` / `1` | ближе какой дальности не резать и сколько фигур выносить из компоненты; одна фигура вместо всех — 6.4 против 9.4 ложных трека на км при той же дальности |
+| `enable_habituation` | `false` | привыкание внутри проезда. Выключено: измерено, что избирательности нет, см. [EXPERIMENTS](docs/EXPERIMENTS.md) п. 10 |
+| `best_effort` | `false` | QoS: RELIABLE. При BEST_EFFORT потеря одного UDP-фрагмента отбрасывает весь 24-мегабайтный кадр |
+| `queue_depth` | `10` | глубина очереди подписки |
+| `raw_subscription` | `true` | брать кадр сырыми байтами CDR, минуя сборку Python-объекта `sensor_msgs` |
+| `async_worker` | `false` | обрабатывать в отдельном потоке; по умолчанию в колбэке — поток борется за GIL с приёмом |
+
+Ничего про геометрию сенсора не захардкожено: решётка лучей, высота установки, крен и тангаж
+**калибруются по самим данным** на первых кадрах. В предоставленных записях встречаются две
+разные раскладки скана (3600 азимутов на 360° и 1200 на 120°) и две высоты установки сенсора
+(1.31 м и 1.70 м) — решение работает с обеими без единой правки.
+
+---
+
+## Результаты
+
+Полный разбор с методикой — [docs/EXPERIMENTS.md](docs/EXPERIMENTS.md).
+
+| Метрика | Значение |
+|---|---|
+| Реальный объект (0.67 × 1.35 м на 55 м) | обнаружен в **99.5 %** кадров |
+| Синтетика организаторов (`cloud_with_fake_obj`, 10 предметов) | **9 из 10** верно: из восьми предметов в габарите найдены семь (2×2 посередине — с 95 м, 2×2 сверху — с 87 м, длинный на рельсах — с 79 м, 0.3 на рельсе — с 76 м, стержень 5 см с потолка — только с 13 м), оба предмета вне габарита — без тревоги; не найден 2×2, заходящий в габарит на 0.3 м. Ложных тревог за 151 с — три коротких (`tools/eval_org_synth.py`, EXPERIMENTS п. 18–19) |
+| Ложные тревоги, leave-one-bag-out | **5.7 разных ложных трека на километр** (8.4 % кадров) — без обученного считывания 11.9 |
+| То же на незнакомой линии, памяти нет | **12.8 на км** (20.7 % кадров) — без считывания 36.1 |
+| То же на второй половине `new_data` — другой день, 3.41 км, ни в каком виде не видена при обучении, памяти нет | **11.7 на км** (17.9 % кадров) |
+| Время обработки кадра | итоговый образ в контейнере: `doubleT_obstacle` **33.4 / 37.9 мс** (медиана / p95), синтетика **43.5 / 48.8 мс** (требование 100 мс) |
+| То же на ядре уровня стенда жюри (i7-9700E ≈ 0.61 нашего по PassMark, `docker/jury_cpu_test.sh`) | реальная запись **55 / 65 мс** (медиана / p95), 201 из 201; синтетика **68 / 80 мс**, 1510 из 1510 |
+| Приём в контейнере | **201 из 201** и **252 из 252**, отброшено **0** |
+| Рабочая дальность, размеченный полигон | **100 м** для человека стоя, **122 м** сидя, **100 м** для чемодана, 20 м для человека лёжа |
+| Обнаружение на 50 м (при видимости) | **0.72** человек стоя; **0.50** человек лёжа — против 0.27 до пола в колее |
+| Обнаружение на 100 м (при видимости) | **0.52** стоя и **0.54** сидя — против 0.19 без обученного считывания; лёжа **0.19** против 0.00 |
+| Обнаружение на 150 м (при видимости) | **0.46** стоя и **0.39** сидя — против 0.39 и 0.20 до пола в колее |
+| Оценка скорости без одометрии | согласие перепроекции 0.76–0.95 |
+| Разделение «знакомое / новое» | ROC AUC 0.905 |
+| Обобщение на форму, которой не было в обучении | ROC AUC **0.965–0.997**, потеря не больше 0.028 |
+| Вклад накопления улик | без него ложных объектов в 13.5 раза больше |
+| Вклад памяти тоннеля | вдвое меньше ложных тревог, обнаружение не страдает |
+| Вклад обученного считывания MBON | при итоговых настройках ложных вдвое меньше: 11.9 → 5.7 на км на знакомой линии, 36.1 → 14.7 на незнакомой |
+| Вклад гашения знакомости на дальности | P@100 0.31 → 0.53, P@150 0.00 → 0.33, рабочая дальность 80 → 100 м |
+| Вклад дальнего порога тревоги | P@150 0.33 → 0.37, человек сидя 0.22 → 0.24; ложных на знакомой линии 8.0 → 8.0, на незнакомой 20.3 → 21.3 |
+| Вклад резкости считывания 1.5 | ложных на незнакомой линии 23.5 → 14.7 на км, на знакомой 6.5 → 5.7, на второй половине `new_data` 23.9 → 17.4; полигон парно +1 / −142 из 7 600 наблюдений (39 вблизи у края габарита, 82 за 90 м). Сравнение при равной строгости с моделью, обученной на `new_data`, — EXPERIMENTS п. 17.4 |
+| Вклад габарита 1.2 м и верхней секции (тогда до 3.7 м, теперь до 3.3 — п. 19) | синтетика организаторов 6 → 8 из 10; полигон парно +43 / −33, посторонних 793 → 742; ложных на незнакомой линии 14.7 → 13.8 на км, на знакомой 5.7 → 5.7; цена — ящик на 100 м (P@100 0.50 → 0.40). EXPERIMENTS п. 18.4 |
+| Вклад сброса при скачке времени | вторая половина `new_data` (70 с выпавших кадров): 17.4 → 13.8 ложного трека на км; на записях без разрывов не меняет ничего. EXPERIMENTS п. 18.9 |
+| Вклад штрафа за плоское у пола и прохода по верхней секции | синтетика 8 → 9 из 10, фантомов на ней 5 → 3; пустые записи 3 → 2 ложных трека (73 → 45 кадров); незнакомая линия 13.8 → 12.8 на км, вторая половина `new_data` 14.4 → 11.7; знакомая 5.7 → 5.7; полигон парно 0 / −3 (каска), посторонних 742 → 758. EXPERIMENTS п. 19 |
+| Вклад пола в колее (с переобучением считывания) | лёжа P@50 0.27 → 0.50, P@150 стоя 0.39 → 0.49, ящик 0.06 → 0.26; парно +685 наблюдений против −33; ложных на знакомой линии 8.0 → 8.0, на незнакомой 21.3 → 23.5 |
+| Вклад разреза по контрасту | 9.1 → 7.5 ложных трека на км *(замер до обученного считывания)*, и слепая полоса 40–90 м на `roundT_pressureGate_roundT` 0.00 → 0.45…1.00 |
+
+Строки «вклад …» для отдельных механизмов измерены до появления обученного
+считывания, и опора у них 7.4–7.5 трека на километр, а не 3.3.
+
+**Про полигон честно.** 23.09 он пересобран: у каждого сценария теперь свой
+генератор случайности, а в каталоге появился человек лёжа. При **тех же**
+настройках человек стоя дал 80 м рабочей дальности и P@100 = 0.48 вместо
+прежних 100 м и 0.53 — это шум полигона, а не поломка: две реализации
+случайности расходятся на P@150 до 0.07. Поэтому правки теперь меряются
+**парно**, на одних и тех же вставках (`tools/compare_benchmark.py`): считается,
+сколько наблюдений перевернулось из «не видел» в «видел» и обратно. Разбор —
+[EXPERIMENTS](docs/EXPERIMENTS.md), п. 16.1.
+
+**Про слепые участки честно.** Размеченный полигон вскрыл то, чего не видно на одном
+реальном объекте: на части участков предмет сливается со стеной по дальности, попадает
+с ней в одну связную компоненту и отбрасывается вместе с ней. Человек на оси пути
+обнаруживался в 100 % кадров на трёх бэгах из пяти и в 0 % на двух — при 65 лучах на
+предмете, то есть не из-за видимости.
+
+**Один из двух слепых бэгов прозрел.** На `roundT_pressureGate_roundT` — той самой
+записи, где компонента из 42 000 лучей течёт вдоль стены от 4 до 99 м и уносит
+предмет с собой, — полностью слепая полоса 40–90 м стала 0.45 / 1.00 / 0.57.
+Помог разрез компоненты по контрасту ламины: гладкая стена даёт нулевой
+центр-окружение по построению, а предмет на ней — ступеньку. Из переглубокой
+компоненты выносится ровно одна, сильнейшая фигура; без этого ограничения разрез
+отрезает фон, протяжённость кандидата падает с 6.9 до 0.8 м, вместе с ней пропадает
+множитель компактности в весе улики, и каждое наблюдение начинает весить вдесятеро
+больше. Измеренная цена одной фигуры против всех — 6.4 против 9.4 ложных трека на км
+при одинаковой дальности. Второй слепой бэг, `roundT_doubleT`, за 40 м остаётся
+слепым, и это разобрано покадрово: за 50 м до предмета **не доходит линия взгляда** —
+98 % лучей упираются в преграду ближе него независимо от того, куда поперёк его
+ставить, — а на 35–52 м мешает наш порог `split_near`, снижать который вышло
+слишком дорого (7.4 → 17.1 ложного трека на км ради девяти метров). Подробно —
+[EXPERIMENTS](docs/EXPERIMENTS.md), п. 9.6.
+
+Два предыдущих подхода к тому же — разрез по допуску глубины и разрез по скорости
+сближения (`split_adv`) — тоже возвращали зрение, но стоили 25.8 и 21.5 трека на
+километр и остались выключенными. Разбор всех трёх — [EXPERIMENTS](docs/EXPERIMENTS.md),
+п. 9.3–9.5.
+
+**Про новый участок честно.** Обученная память на незнакомой линии бесполезна по
+определению: без неё ложных треков 21.8 на километр против 9.1. Мы попробовали
+закрыть это привыканием внутри проезда — гасить форму, встретившуюся в нескольких
+разных точках пути, то есть штатную повторяющуюся обстановку. Механизм сделан,
+доведён до работы и **отвергнут по замеру**: при ёмкости, достаточной чтобы
+популяция не насыщалась, он не меняет ничего (7.5 против 7.5 ложных треков на км),
+а весь видимый эффект маленькой популяции оказался глобальным глушением, которое
+давит предмет сильнее обстановки (новизна вставленного предмета 0.24 при медианной
+новизне кандидата 0.43…0.71). Код оставлен и выключен, полный разбор с таблицами —
+[EXPERIMENTS](docs/EXPERIMENTS.md), п. 10.
+
+Работает на новом участке то, что и работало: перенос долговременной памяти. В
+дескрипторе намеренно смешаны признаки формы и углового размера (переносятся на
+любой тоннель) с положением в сечении (запоминает конкретную обстановку), и первая
+половина снижает ложные тревоги с 21.8 до 9.1 трека на километр на бэге, которого
+память не видела.
+
+**Про обученное считывание честно.** Метки для него сделаны вставкой предметов
+трассировкой лучей, и первые две модели сенсора оказались неверными: сначала
+яркость вставки считалась по ламбертовой ρ·cosθ/r², и за 110 м предмет выходил
+тусклее тоннеля, потом — постоянной, и он стал ярче тоннеля впятеро. В обоих
+случаях модель училась узнавать вставку по яркости, а не по форме, и полигонная
+дальность была завышена. Абсолютной шкалы интенсивности в этих записях нет вовсе:
+медиана по кандидатам обстановки 3…7 в пяти бэгах и 23.5 в шестом. Сейчас вставка
+берёт яркость реальных возвратов с тех же лучей, признак стал неинформативным, и
+все цифры выше получены уже так. Заявленные до этого разбора 100 м рабочей
+дальности и P@100 = 0.53 были получены на полигоне с артефактом: после починки
+модели сенсора те же замеры дали 80 м и 0.31. Нынешние 100 м и 0.53 —
+совпадение по величине, но получены они уже на честном полигоне и другими
+средствами (гашение знакомости и порог по лучам, зависящий от дальности).
+Разбор с таблицами — [EXPERIMENTS](docs/EXPERIMENTS.md), п. 11.
+
+**Про дальность и ложные честно.** Улика далёкого предмета — произведение
+нескольких множителей, и один из них на дальности оказался перевёрнутым:
+у вставленного человека на 120–185 м новизна 0.150 против 0.199 у окружающей
+обстановки (AUC 0.293). На шести лучах дескриптор вырождается, и память
+тоннеля узнаёт в предмете любую далёкую конструкцию — то есть мы сами гасили
+свой сигнал. После гашения вклада новизны за 90 м обнаружение на 150 м
+выросло с нуля до 0.33, а на 100 м с 0.31 до 0.53. Вместе с порогом по числу
+лучей, зависящим от дальности (четыре вблизи, три за 90 м), рабочая дальность
+по человеку выросла с 80 до 100 м. Цена — ложные тревоги на **знакомой**
+линии 3.5 → 8.0 трека на километр; на незнакомой плата нулевая, потому что
+там подавлять нечем. Кому дороже тишина, тот ставит `nov_fade_from: 0` и
+получает 3.5 на километр, теряя дальнюю зону. Лишние далёкие треки дают предупреждения, а
+не торможение: экстренный уровень требует близкой дистанции. Разбор —
+[EXPERIMENTS](docs/EXPERIMENTS.md), п. 12.
+
+**Про решение по треку честно.** Замышлялось обученное считывание по истории
+трека — и оно **не обогнало ни один из признаков, которые ему же и дали**
+(AUC 0.880 против 0.895 у одного среднего отсчёта). Зато по дороге нашлось, что
+улика насыщается: на настоящем объекте она 1.000 и у предмета, и у ложных
+треков (AUC 0.624), а средний вес наблюдения, из которого она складывается, —
+0.998 против 0.269. Смешивать его с уликой оказалось бесполезно: при равном
+числе ложных тревог простой порог даёт обнаружение не хуже, а на незнакомой
+линии смешивание вытаскивает лишние треки (20.3 → 24.0 на км). В итоге приняли
+не модель, а ключ, **отвергнутый тремя разделами раньше**, — порог тревоги,
+опускаемый с 0.5 до 0.3 за 90 м. Тогда он ничего не давал, потому что улика
+далёкого трека была нулём; после гашения знакомости она им быть перестала, и
+тот же ключ поднял P@150 с 0.33 до 0.37. Отрицательный результат верен только
+для конфигурации, в которой получен. Разбор — [EXPERIMENTS](docs/EXPERIMENTS.md),
+п. 15.
+
+**Про упавшего на пути человека честно.** Самый важный для метро случай был
+виден хуже всех крупных предметов: при высоте 0.30 м и поле габарита 0.28 м в
+габарит попадала верхушка в два сантиметра (P@50 = 0.27, на 100 м — ноль). Пол
+между рельсами опущен до 0.16 м (предложение Zhirik1337), но не везде: без
+порога по дальности обнаружение **вблизи падало вдвое** — в полосу 0.16…0.28 м
+попадают головки рельсов, рельс собирается в одну компоненту от самой кабины, и
+предмет выбрасывается вместе с ней. Дальше 30 м вреда нет, и после
+переобучения считывания на кандидатах нового пола лежачий человек P@50 = 0.50,
+рабочая дальность 32 м; ложных на знакомой линии столько же, на незнакомой
++2.2 на км. Ещё четыре правки из того же набора замерены и отвергнуты: каждая
+давала меньше, чем стоила. Разбор — [EXPERIMENTS](docs/EXPERIMENTS.md), п. 16.
+
+**Про ось пути честно.** Ось берётся не из рельсов, а из дрейфа центра сечения
+тоннеля с дальностью: на 100 м рельсы дают единицы точек, а свод — тысячи.
+Поэтому ось не дрожит: сдвиг между соседними кадрами в одной точке пути на
+100 м — p90 не хуже 0.35 м при полуширине габарита 1.6 м. Но наблюдается
+сечение только до 62–107 м, дальше ось продолжается по касательной, и
+касательная расходится с кривой до 1.19 м на 150 м. Продолжать вместо неё
+измеренную кривизну пробовали — хуже: кривизна оценивается на коротком плече,
+гуляет между кадрами и даёт до 6 м расхождения.
+
+**Про край габарита честно.** На синтетике организаторов граница между «у края, внутри» и
+«вне, но близко» — сантиметры: 1.13 и 1.14 м от оси. Ошибка нашей оси на 40–80 м —
+0.2–0.4 м, проверено по их предметам (EXPERIMENTS п. 20.1). Поэтому ящик 2×2, заходящий в
+габарит на 0.06–0.28 м, мы не берём: по форме и перепаду дальности он неотличим от
+настоящих конструкций у края, которые из-за той же ошибки оси оказываются у нас на
+1.0–1.1 м. Правило, которое его бы нашло, на записях без препятствий срабатывает в 16
+местах, одно держится 451 кадр (п. 20.3).
+
+Из этого следует оговорка к дальним цифрам полигона. Предмет вставляется на
+**нашу же** оценку оси, поэтому ошибку оси полигон не мерит в принципе. В
+эксплуатации предмет на 150 м может оказаться в метре от того места, где мы
+считаем путь, и у края габарита из него выпасть. Дальние цифры оптимистичны
+именно по этой причине, а не из-за обнаружения. Разбор —
+[EXPERIMENTS](docs/EXPERIMENTS.md), п. 13.
+
+**Про дальность честно.** Паспортный максимум Pandar128 — 200 м, максимальное эхо в датасете —
+208.8 м. Но реальная **прямая видимость в этих тоннелях 121–167 м**: тоннели кривые (радиусы
+1300–8700 м), и дальше линия взгляда упирается в стену. Заявленные в ТЗ «300 м → отлично» на
+предоставленных участках физически недостижимы никаким алгоритмом.
+
+Что мешает дотянуться до 200 м там, где видимость позволяет, тоже измерено и оказалось не
+тем, чего ожидаешь. Человек на 170 м освещён **в каждом кадре** (5 лучей) и за проход набирает
+около 84 попаданий в одну точку мира; кандидат формируется, размер определяется верно. Не
+растёт улика: кольцо окружения ламины на 170 м упирается в стену тоннеля, которая там же, и
+локальный контраст обнуляется. Лечится не порогом, а накоплением лучей в координатах пути и
+геометрической картой линии — разбор в [docs/EXPERIMENTS.md](docs/EXPERIMENTS.md), п. 7.3.
+
+**Про стекло честно.** Лидар светит на 905 нм, и прозрачное стекло для него почти
+прозрачно: луч уходит насквозь и возвращается от того, что за стеклом, а гладкая
+поверхность отражает зеркально — в сторону, а не назад к прибору. Стеклянный предмет
+виден только тем, что в нём не прозрачно: этикеткой, пробкой, рамой, грязью, бликом
+там, где луч падает на поверхность почти по нормали. Отдельного приёма для стекла у нас
+нет, и честно его не сделать: из пустоты предмет не восстановить. Для масштаба — даже
+непрозрачная бутылка (0.03 м²) на полигоне почти не видна: на 40–55 м на ней 3 луча, а
+ниже примерно десяти лучей предмет не отличить от шума (EXPERIMENTS п. 9).
+
+---
+
+## Разработка без ROS
+
+Весь конвейер работает и офлайн, прямо по `.db3`, без установленного ROS — это удобно для
+экспериментов на Windows и для воспроизведения метрик:
+
+```bash
+python tools/inspect_bags.py --root data/for_hackathon # калибровка решётки
+python tools/run_pipeline.py --all --memory artifacts/mushroom_body.npz
+python tools/evaluate.py --device cuda # leave-one-bag-out
+python tools/make_benchmark.py --memory artifacts/mushroom_body.npz
+python tools/render_brain.py --bag data/for_hackathon/doubleT_obstacle --video brain.mp4
+```
+
+`evaluate.py`, `make_benchmark.py` и `make_training_set.py` раскладывают работу по
+бэгам на процессы (`--jobs`, по умолчанию — по числу записей, но не больше
+физических ядер). Результат совпадает с последовательным побайтово: случайность
+у каждой записи своя, общей изменяемой памяти между ними нет. Задержку кадра при
+этом мерить нельзя — нужен `--jobs 1`.
+
+Обучение памяти тоннеля (без единой метки, на пустых проездах):
+
+```bash
+python tools/train_mushroom_body.py --device cuda \
+ --extra-cache data/cache/new_data_candidates.npz
+```
+
+---
+
+## Состав репозитория
+
+```
+ros2_ws/src/flyguard/ ROS 2-пакет: конвейер, узел, launch, конфиги, RViz, вид мозга
+ros2_ws/src/flyguard_msgs/ сообщения ObstacleStatus и DetectedObject
+docker/ Dockerfile образа, точка входа, проверки в контейнере
+flyguard/ то же ядро, что в ROS-пакете, — для инструментов и тестов без ROS
+tools/ офлайн-инструменты: калибровка, обучение, метрики, полигон
+tests/ тесты без ROS: pytest tests
+docs/ архитектура, алгоритм, эксперименты, коннектом
+artifacts/ обученная память тоннеля и результаты замеров
+```
+
+Ядро в `flyguard/` и в `ros2_ws/src/flyguard/flyguard/` — один и тот же код из одного
+источника. Сборка и запуск по этому README — через `docker/Dockerfile`. В корне лежат ещё
+`Dockerfile`, `Dockerfile.gpu` и `docker-compose.yml` для прогона ядра по записям без ROS
+и `Dockerfile.ros2` с узлом `flyguard/flyguard_ros2_node.py`.
+
+## Лицензия
+
+MIT.
diff --git a/artifacts/ablation.json b/artifacts/ablation.json
new file mode 100644
index 0000000..980076a
--- /dev/null
+++ b/artifacts/ablation.json
@@ -0,0 +1,37 @@
+[
+ {
+ "variant": "полная система",
+ "alarm_rate": 0.10717703349282297,
+ "fp_tracks": 6,
+ "obj_rate": 0.9894736842105263,
+ "seconds": 70.2
+ },
+ {
+ "variant": "− память тоннеля",
+ "alarm_rate": 0.24114832535885167,
+ "fp_tracks": 16,
+ "obj_rate": 0.9894736842105263,
+ "seconds": 61.4
+ },
+ {
+ "variant": "− ось пути (прямой коридор)",
+ "alarm_rate": 0.04401913875598086,
+ "fp_tracks": 3,
+ "obj_rate": 0.9894736842105263,
+ "seconds": 54.6
+ },
+ {
+ "variant": "− признаки формы",
+ "alarm_rate": 0.30526315789473685,
+ "fp_tracks": 21,
+ "obj_rate": 0.9894736842105263,
+ "seconds": 61.0
+ },
+ {
+ "variant": "− накопление улик",
+ "alarm_rate": 0.3741626794258373,
+ "fp_tracks": 81,
+ "obj_rate": 1.0,
+ "seconds": 60.9
+ }
+]
\ No newline at end of file
diff --git a/artifacts/track_readout.npz b/artifacts/track_readout.npz
new file mode 100644
index 0000000..f6697dc
Binary files /dev/null and b/artifacts/track_readout.npz differ
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..7aac403
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,109 @@
+# FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
+#
+# Образ самодостаточен: во время работы сеть не нужна, все зависимости ставятся
+# на этапе сборки. Базовый образ — официальный ROS 2 Humble на Ubuntu 22.04,
+# ровно та среда, что указана в ТЗ.
+#
+# docker build -t flyguard -f docker/Dockerfile .
+# docker run --rm -it --gpus all --network host --ipc host -v /path/to/bags:/data flyguard
+#
+# Без --gpus all (или на машине без видеокарты NVIDIA) тот же образ считает на
+# процессоре — с тем же результатом.
+FROM ros:humble-ros-base-jammy
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Численное ядро берётся из репозиториев Ubuntu, а не из pip: так образ собирается
+# без выхода в PyPI и получает numpy/scipy, слинкованные с системным BLAS.
+RUN apt-get update && apt-get install -y --no-install-recommends \
+ python3-numpy \
+ python3-scipy \
+ python3-opencv \
+ python3-pil \
+ fonts-dejavu-core \
+ python3-colcon-common-extensions \
+ ros-humble-rviz2 \
+ ros-humble-rosbag2-storage-mcap \
+ ros-humble-diagnostic-msgs \
+ && rm -rf /var/lib/apt/lists/*
+
+# PyTorch с CUDA 12.8 — плотные стадии конвейера на видеокарте (сетчатка,
+# ламина, кластеризация; flyguard/gpu.py). Колёса несут свою среду CUDA, от
+# машины нужен только драйвер NVIDIA (у стенда 580 — это до CUDA 13.0).
+# 2.9.1 — последняя сборка под Python 3.10 из Ubuntu 22.04; CUDA 12.8 нужна
+# для Blackwell (RTX 50xx) и годится для Ada (RTX 40xx). Весит PyTorch с
+# библиотеками CUDA 6.5 ГБ, поэтому ставится одним слоем и без кэша pip.
+#
+# Колёса — из индекса PyTorch, а если задан TORCH_WHEELS — с этого адреса
+# (сборка там, где сети нет: колёса скачиваются на соседней машине и отдаются
+# любым HTTP-сервером, docker/wheels/README.md). В контекст сборки колёса не
+# кладутся: это ещё 4 ГБ копий на диске.
+ARG TORCH_WHEELS=
+RUN set -e; \
+ if [ -n "$TORCH_WHEELS" ]; then \
+ python3 -c "import re, urllib.request as u; b = '$TORCH_WHEELS'.rstrip('/'); \
+f = re.findall(r'href=\"(pip-[^\"]+\.whl)\"', u.urlopen(b + '/').read().decode())[0]; \
+u.urlretrieve(b + '/' + f, '/tmp/pip.whl')"; \
+ python3 /tmp/pip.whl/pip install --no-cache-dir --no-index \
+ --find-links "$TORCH_WHEELS" torch==2.9.1; \
+ rm -f /tmp/pip.whl; \
+ else \
+ apt-get update && apt-get install -y --no-install-recommends python3-pip; \
+ pip3 install --no-cache-dir --index-url https://download.pytorch.org/whl/cu128 \
+ torch==2.9.1; \
+ rm -rf /var/lib/apt/lists/*; \
+ fi; \
+ python3 -c "import torch; print('torch', torch.__version__, 'CUDA', torch.version.cuda)"
+
+# Видеокарта для CUDA и для RViz (OpenGL через драйвер NVIDIA при --gpus all)
+ENV NVIDIA_VISIBLE_DEVICES=all \
+ NVIDIA_DRIVER_CAPABILITIES=all
+
+WORKDIR /opt/flyguard
+
+# Сначала манифесты — слой с зависимостями переживает правки кода
+COPY ros2_ws/src/flyguard/package.xml ros2_ws/src/flyguard/package.xml
+COPY ros2_ws/src/flyguard_msgs/package.xml ros2_ws/src/flyguard_msgs/package.xml
+COPY ros2_ws/src/flyguard_msgs/CMakeLists.txt ros2_ws/src/flyguard_msgs/CMakeLists.txt
+
+COPY ros2_ws/src ros2_ws/src
+# Память тоннеля и обученное считывание MBON. Без считывания узел работает
+# на ручной формуле веса улики, а у неё ложных тревог в полтора раза больше
+# (13.6 против 8.0 на км, README) — замеренная конфигурация и образ должны совпадать.
+COPY artifacts/mushroom_body.npz ros2_ws/src/flyguard/artifacts/mushroom_body.npz
+COPY artifacts/mbon_readout.npz ros2_ws/src/flyguard/artifacts/mbon_readout.npz
+
+# Обычная установка, без --symlink-install: тот режим оставляет в install/
+# ссылки внутрь build/ и src/, и последующее удаление build/ ломает пакет.
+RUN . /opt/ros/humble/setup.sh \
+ && cd ros2_ws \
+ && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
+ && rm -rf build log
+
+COPY docker/entrypoint.sh /entrypoint.sh
+COPY docker/fastdds_large.xml /opt/flyguard/fastdds_large.xml
+COPY docker/fastdds_udp.xml /opt/flyguard/fastdds_udp.xml
+# Пульт проигрывания для `docker exec` (пауза, перемотка, скорость)
+COPY docker/flyguard-keys.sh /usr/local/bin/flyguard-keys
+# Снятие CR: при клонировании на Windows git может подставить CRLF, и тогда
+# ядро ищет интерпретатор «bash\r». Ошибка неочевидная, а стоит одной строки.
+RUN sed -i 's/\r$//' /entrypoint.sh /usr/local/bin/flyguard-keys \
+ && chmod +x /entrypoint.sh /usr/local/bin/flyguard-keys
+
+# BLAS в один поток. Матрицы конвейера мелкие, и потоки OpenBLAS не ускоряют
+# кадр, а крутятся вхолостую в ожидании работы: при 4 потоках узел занимал
+# 3 ядра, при одном — 0.4 ядра, а кадр даже чуть быстрее (33.9 / 39.3 мс против
+# 33.2 / 40.5, медиана / p95; синтетика 43.7 / 50.2 против 45.9 / 54.7).
+ENV OMP_NUM_THREADS=1 \
+ OPENBLAS_NUM_THREADS=1 \
+ MKL_NUM_THREADS=1 \
+ PYTHONUNBUFFERED=1 \
+ RCUTILS_COLORIZED_OUTPUT=1
+
+# Транспорт под кадр в 24 МБ: умолчания Fast DDS рассчитаны на сотни килобайт
+# и теряли половину кадров ещё до подписчика. Профиль поднимает сегмент
+# разделяемой памяти и буферы сокетов; снимается через FLYGUARD_DDS_PROFILE=0.
+ENV FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_large.xml
+
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["ros2", "launch", "flyguard", "detect.launch.py"]
diff --git a/docker/bagrate_test.sh b/docker/bagrate_test.sh
new file mode 100644
index 0000000..2756d6f
--- /dev/null
+++ b/docker/bagrate_test.sh
@@ -0,0 +1,67 @@
+#!/usr/bin/env bash
+# С какой частотой `ros2 bag play` реально отдаёт кадры — без нашей ноды.
+# Нужно, чтобы отличить потери в нашем коде от предела самого транспорта.
+set -u
+BAG="${1:?укажите бэг}"
+TOPIC="${2:-/sensing/lidar/hesai128/pointcloud}"
+
+python3 - "$TOPIC" > /tmp/rate.txt 2>/dev/null <<'PY' &
+import sys, time
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy,
+ QoSDurabilityPolicy)
+from sensor_msgs.msg import PointCloud2
+
+topic = sys.argv[1]
+qos = QoSProfile(history=QoSHistoryPolicy.KEEP_LAST, depth=10,
+ reliability=QoSReliabilityPolicy.RELIABLE,
+ durability=QoSDurabilityPolicy.VOLATILE)
+
+
+class Sink(Node):
+ def __init__(self):
+ super().__init__("sink")
+ self.n = 0
+ self.first = self.last = None
+ self.bytes = 0
+ self.create_subscription(PointCloud2, topic, self.cb, qos, raw=True)
+
+ def cb(self, raw):
+ now = time.perf_counter()
+ if self.first is None:
+ self.first = now
+ self.last = now
+ self.n += 1
+ self.bytes += len(raw)
+
+
+rclpy.init()
+s = Sink()
+try:
+ rclpy.spin(s)
+except KeyboardInterrupt:
+ pass
+finally:
+ span = (s.last - s.first) if (s.first and s.last and s.last > s.first) else 0
+ print(f"принято сообщений: {s.n}")
+ if span > 0:
+ print(f"за {span:.1f} с → {s.n / span:.2f} Гц")
+ print(f"поток: {s.bytes / span / 1e6:.0f} МБ/с, размер кадра "
+ f"{s.bytes / max(s.n, 1) / 1e6:.1f} МБ")
+PY
+SINK=$!
+sleep 3
+
+echo "=== проигрывание (приёмник только считает, ничего не обрабатывает) ==="
+T0=$(date +%s.%N)
+ros2 bag play "$BAG" >/dev/null 2>&1
+T1=$(date +%s.%N)
+awk -v a="$T0" -v b="$T1" 'BEGIN{printf "бэг проигран за %.1f с
+", b-a}'
+
+sleep 2
+kill -INT $SINK 2>/dev/null || true
+wait $SINK 2>/dev/null || true
+echo
+cat /tmp/rate.txt
diff --git a/docker/brain_test.sh b/docker/brain_test.sh
new file mode 100644
index 0000000..ff37299
--- /dev/null
+++ b/docker/brain_test.sh
@@ -0,0 +1,71 @@
+#!/usr/bin/env bash
+# Проверка топика /flyguard/brain: он публикуется и содержит настоящую картинку.
+# Несколько кадров сохраняются в PNG — их же можно показать в презентации.
+#
+# docker run --rm --network host --shm-size=1g -v <бэги>:/data \
+# -v <репо>/docker:/t:ro -v <куда>:/out flyguard bash /t/brain_test.sh /data/<бэг>
+set -u
+BAG="${1:?укажите путь к бэгу}"
+OUT="${OUT:-/out}"
+mkdir -p "$OUT"
+
+ros2 launch flyguard detect.launch.py brain_view:=true > /tmp/node.log 2>&1 &
+LAUNCH_PID=$!
+for i in $(seq 1 30); do
+ ros2 topic list 2>/dev/null | grep -q '^/flyguard/brain$' && break
+ sleep 1
+done
+echo "топик /flyguard/brain объявлен через ${i} с"
+
+python3 - "$OUT" > /tmp/brain.txt 2>&1 <<'PY' &
+import sys, rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+import numpy as np, cv2
+
+out = sys.argv[1]
+
+
+class Grab(Node):
+ def __init__(self):
+ super().__init__("brain_grab")
+ self.n = 0
+ self.shapes = set()
+ self.create_subscription(Image, "/flyguard/brain", self.cb, 10)
+
+ def cb(self, m):
+ self.n += 1
+ a = np.frombuffer(m.data, np.uint8).reshape(m.height, m.width, -1)
+ self.shapes.add((m.height, m.width, m.encoding, a.shape[2]))
+ # берём каждый двадцатый кадр, чтобы картинки отличались
+ if self.n % 20 == 1 and self.n < 200:
+ img = a if m.encoding == "bgr8" else cv2.cvtColor(a, cv2.COLOR_RGB2BGR)
+ cv2.imwrite(f"{out}/brain_{self.n:04d}.png", img)
+
+
+rclpy.init()
+g = Grab()
+try:
+ rclpy.spin(g)
+except KeyboardInterrupt:
+ pass
+finally:
+ print(f"кадров схемы принято: {g.n}")
+ for s in g.shapes:
+ print(f"размер {s[1]}x{s[0]}, кодировка {s[2]}, каналов {s[3]}")
+PY
+GRAB_PID=$!
+sleep 2
+
+ros2 bag play --read-ahead-queue-size 10 "$BAG" > /dev/null 2>&1
+
+sleep 2
+kill -INT $GRAB_PID 2>/dev/null || true
+wait $GRAB_PID 2>/dev/null || true
+kill $LAUNCH_PID 2>/dev/null || true
+sleep 1
+
+echo
+cat /tmp/brain.txt
+echo
+ls -la "$OUT" | head -15
diff --git a/docker/check_all.sh b/docker/check_all.sh
new file mode 100644
index 0000000..32b8960
--- /dev/null
+++ b/docker/check_all.sh
@@ -0,0 +1,23 @@
+#!/usr/bin/env bash
+# Все записи подряд: для каждой — узел, проигрывание и строка сводки.
+#
+# docker run --rm --network host --ipc host -v <бэги>:/data:ro \
+# -v <репо>/docker:/smoke:ro flyguard bash /smoke/check_all.sh
+#
+# Узел поднимается заново на каждую запись: это разные проезды, и состояние
+# одного (треки, ось пути) не должно перетекать в другой. «Кадров с тревогой»
+# на записи без препятствия — это ложные тревоги.
+set -u
+here="$(dirname "$0")"
+printf '%-38s %8s %9s %22s %26s\n' "запись" "принято" "обработ." "кадров с тревогой" "кадр: медиана / p95, мс"
+for bag in /data/*/; do
+ [ -f "$bag/metadata.yaml" ] || continue
+ name=$(basename "$bag")
+ out=$(bash "$here/demo_test.sh" "$bag" 2>&1)
+ recv=$(echo "$out" | grep -o "frames_received=[0-9]*" | cut -d= -f2)
+ proc=$(echo "$out" | grep -o "frames_processed=[0-9]*" | cut -d= -f2)
+ hits=$(echo "$out" | grep -o "кадров с обнаружением: [0-9]* ([0-9.]*%)" | sed 's/кадров с обнаружением: //')
+ ms=$(echo "$out" | grep -o "медиана [0-9.]* мс, p95 [0-9.]*" | sed 's/медиана //; s/ мс, p95 / \/ /')
+ first=$(echo "$out" | grep -o "ПРЕПЯТСТВИЕ: [0-9.]* м" | head -1)
+ printf '%-38s %8s %9s %22s %26s %s\n' "$name" "${recv:--}" "${proc:--}" "${hits:-0}" "${ms:--}" "$first"
+done
diff --git a/docker/ddsrate_test.sh b/docker/ddsrate_test.sh
new file mode 100644
index 0000000..7274841
--- /dev/null
+++ b/docker/ddsrate_test.sh
@@ -0,0 +1,14 @@
+#!/usr/bin/env bash
+# Сравнение транспорта DDS: умолчания против профиля для крупных сообщений.
+# Приёмник ничего не обрабатывает, поэтому меряется именно транспорт.
+set -u
+BAG="${1:?укажите бэг}"
+TOPIC="${2:-/sensing/lidar/hesai128/pointcloud}"
+PROFILE="${FASTRTPS_DEFAULT_PROFILES_FILE:-<умолчания>}"
+
+echo "профиль: $PROFILE"
+echo "/dev/shm: $(df -h /dev/shm | awk 'NR==2{print $2}')"
+echo "rmem_max: $(cat /proc/sys/net/core/rmem_max)"
+echo
+
+bash "$(dirname "$0")/bagrate_test.sh" "$BAG" "$TOPIC"
diff --git a/docker/demo_test.sh b/docker/demo_test.sh
new file mode 100644
index 0000000..2f6ffa6
--- /dev/null
+++ b/docker/demo_test.sh
@@ -0,0 +1,135 @@
+#!/usr/bin/env bash
+# Сквозная проверка внутри контейнера: нода + проигрывание бэга + разбор вывода.
+#
+# docker run --rm --network host -v <бэги>:/data -v <репо>/docker:/smoke:ro flyguard \
+# bash /smoke/demo_test.sh /data/doubleT_obstacle
+set -u
+# Управление заданиями: без него фоновые процессы неинтерактивного bash глухи к
+# SIGINT, и узел не гас по `kill -INT` — его добивал `kill -9`, без итога.
+set -m
+BAG="${1:?укажите путь к бэгу}"
+shift || true
+LAUNCH_ARGS=("$@")
+
+# NODE=external — узел уже запущен снаружи (другим контейнером, см.
+# jury_cpu_test.sh): здесь только проигрывание и подсчёт.
+LAUNCH_PID=""
+if [ "${NODE:-}" != "external" ]; then
+ echo "=== запуск ноды ==="
+ ros2 launch flyguard detect.launch.py "${LAUNCH_ARGS[@]}" > /tmp/node.log 2>&1 &
+ LAUNCH_PID=$!
+ echo "параметры: ${LAUNCH_ARGS[*]:-по умолчанию}"
+else
+ : > /tmp/node.log
+fi
+for i in $(seq 1 30); do
+ if ros2 topic list 2>/dev/null | grep -q '^/flyguard/obstacle$'; then
+ echo "топик /flyguard/obstacle появился за ${i} с"
+ break
+ fi
+ sleep 1
+done
+# Видеокарта поднимается в фоне, и узел принимает кадры, не дожидаясь её. Для
+# замера скорости ждём её здесь, иначе первые кадры посчитает процессор:
+# решения те же, а время кадра смешается. WAIT_GPU=0 — не ждать, как на стенде.
+if [ "${WAIT_GPU:-1}" = 1 ] && grep -q "вычисления: пока процессор" /tmp/node.log 2>/dev/null; then
+ for i in $(seq 1 90); do
+ grep -qE "вычисления: (видеокарта|процессор \()|не поднялась" /tmp/node.log && break
+ sleep 1
+ done
+ GPU_STATE=$(grep -oE 'готова через [0-9.]+ с|не поднялась|CUDA недоступна' /tmp/node.log | head -1)
+ echo "видеокарта: ${GPU_STATE:-нет ответа за 90 с}"
+fi
+
+# Счётчик на rclpy: `ros2 topic echo` сам не успевает за потоком и теряет
+# сообщения, из-за чего занижает оценку частоты.
+python3 - > /tmp/count.txt 2>/dev/null <<'PY' &
+import rclpy
+from rclpy.node import Node
+from flyguard_msgs.msg import ObstacleStatus
+from diagnostic_msgs.msg import DiagnosticArray
+
+
+class Counter(Node):
+ def __init__(self):
+ super().__init__("counter")
+ self.n = self.hits = 0
+ self.dist, self.ms, self.diag = [], [], {}
+ self.create_subscription(ObstacleStatus, "/flyguard/obstacle", self.on_obs, 50)
+ self.create_subscription(DiagnosticArray, "/flyguard/diagnostics", self.on_diag, 50)
+
+ def on_obs(self, m):
+ self.n += 1
+ self.ms.append(m.processing_ms)
+ if m.detected:
+ self.hits += 1
+ self.dist.append(m.distance)
+
+ def on_diag(self, m):
+ for st in m.status:
+ for kv in st.values:
+ if kv.key in ("dropped_frames", "frames_processed",
+ "frames_received", "frames_calibration",
+ "cycle_ms"):
+ self.diag[kv.key] = kv.value
+
+
+rclpy.init()
+c = Counter()
+try:
+ rclpy.spin(c)
+except KeyboardInterrupt:
+ pass
+finally:
+ import statistics
+ ms = sorted(c.ms)
+ print(f"сообщений: {c.n}")
+ if c.n:
+ print(f"кадров с обнаружением: {c.hits} ({c.hits / c.n:.1%})")
+ if c.dist:
+ print(f"дистанция: медиана {statistics.median(c.dist):.1f} м, "
+ f"мин {min(c.dist):.1f} м, макс {max(c.dist):.1f} м")
+ if ms:
+ print(f"обработка кадра: медиана {statistics.median(ms):.1f} мс, "
+ f"p95 {ms[int(0.95 * (len(ms) - 1))]:.1f} мс, макс {max(ms):.1f} мс")
+ print("по данным самой ноды: " + ", ".join(f"{k}={v}" for k, v in sorted(c.diag.items())))
+PY
+COUNTER_PID=$!
+sleep 2
+
+echo "=== проигрывание бэга ==="
+# read-ahead-queue-size: проигрыватель по умолчанию набивает очередь на 1000
+# сообщений ДО старта. При кадре в 24 МБ это 24 ГБ, чтение занимает секунды,
+# а часы записи уже идут — всё, что за это время «просрочилось», не публикуется.
+PLAY_ARGS="${PLAY_ARGS:---read-ahead-queue-size 10}"
+echo "аргументы проигрывания: $PLAY_ARGS"
+T0=$(date +%s.%N)
+ros2 bag play $PLAY_ARGS "$BAG" 2>&1 | grep -Ei "duration|Opened database" | head -3
+T1=$(date +%s.%N)
+awk -v a="$T0" -v b="$T1" 'BEGIN{printf "проиграно за %.1f с" ORS, b-a}'
+
+sleep 3
+kill -INT $COUNTER_PID 2>/dev/null || true
+wait $COUNTER_PID 2>/dev/null || true
+# SIGINT, а не SIGTERM: так launch гасит и узел, а узел печатает итог. От
+# SIGTERM детектор переживал launch, и в check_all.sh узлы копились от записи
+# к записи, считая кадры соседей и отбирая у них процессор. `kill -9` ниже —
+# только страховка: с `set -m` узел гаснет за полсекунды.
+if [ -n "$LAUNCH_PID" ]; then
+ kill -INT $LAUNCH_PID 2>/dev/null || true
+ for i in $(seq 1 10); do pgrep -f "lib/flyguard/detector" >/dev/null || break; sleep 1; done
+ pkill -9 -f "lib/flyguard/detector" 2>/dev/null || true
+ wait $LAUNCH_PID 2>/dev/null || true
+fi
+
+echo
+echo "=== результат ==="
+cat /tmp/count.txt
+
+echo
+echo "=== журнал ноды ==="
+grep -Ei "вычисления|видеокарт|память|считывание|решётк|кадр лидара|подписка|итог|ошибк|error|traceback" /tmp/node.log | head -20
+
+echo
+echo "=== что узел сказал в консоль ==="
+grep -E "ПРЕПЯТСТВИЕ|препятствие|ЭКСТРЕННОЕ|экстренное|путь свободен" /tmp/node.log | head -30
diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh
new file mode 100644
index 0000000..a817f31
--- /dev/null
+++ b/docker/entrypoint.sh
@@ -0,0 +1,60 @@
+#!/usr/bin/env bash
+# Точка входа контейнера: окружение ROS + прогрев конвейера.
+set -e
+
+source /opt/ros/humble/setup.bash
+source /opt/flyguard/ros2_ws/install/setup.bash
+
+# Профиль транспорта можно снять для сравнения: FLYGUARD_DDS_PROFILE=0
+if [ "${FLYGUARD_DDS_PROFILE:-1}" = "0" ]; then
+ unset FASTRTPS_DEFAULT_PROFILES_FILE
+fi
+
+# Контейнер без --ipc host. Docker монтирует ему свою /dev/shm с источником
+# «shm», а с --ipc host видна хостовая. С --network host Fast DDS считает
+# контейнер и хост одной машиной и шлёт кадры через /dev/shm, но сегмент
+# собеседника отсюда не виден — и кадры пропадают молча: от `ros2 bag play`
+# на хосте узел не получает НИ ОДНОГО (замерено, и с нашим профилем, и без
+# него). В таком контейнере транспорт переводится на UDP: тогда работают
+# проигрывание внутри контейнера и второй контейнер из этого же образа.
+# Проигрыватель на хосте со своими умолчаниями всё равно пойдёт через память,
+# и для него выход один — запускать контейнер с --ipc host.
+shm_source=$(awk '$5 == "/dev/shm" { for (i = 7; i <= NF; i++) if ($i == "-") { print $(i + 2); exit } }' \
+ /proc/self/mountinfo 2>/dev/null || true)
+if [ -n "${FASTRTPS_DEFAULT_PROFILES_FILE:-}" ] && [ "$shm_source" = "shm" ]; then
+ export FASTRTPS_DEFAULT_PROFILES_FILE=/opt/flyguard/fastdds_udp.xml
+ echo "ВНИМАНИЕ: контейнер запущен без --ipc host — транспорт переключён на UDP." >&2
+ echo " Проигрывание внутри контейнера работает, но ros2 bag play на хосте" >&2
+ echo " кадры сюда не доставит: для него запускайте с --ipc host." >&2
+fi
+
+# Сегмент разделяемой памяти не поместится в /dev/shm размером 64 МБ, который
+# Docker даёт по умолчанию, и Fast DDS молча свалится обратно на UDP.
+if [ "${FASTRTPS_DEFAULT_PROFILES_FILE:-}" = "/opt/flyguard/fastdds_large.xml" ]; then
+ shm_mb=$(df -m /dev/shm 2>/dev/null | awk 'NR==2{print $2}')
+ if [ -n "$shm_mb" ] && [ "$shm_mb" -lt 512 ]; then
+ echo "ВНИМАНИЕ: /dev/shm всего ${shm_mb} МБ — запустите с --shm-size=1g," >&2
+ echo " иначе крупные кадры пойдут через UDP и часть потеряется." >&2
+ fi
+fi
+
+# Прогрев: первый вызов numpy/scipy тянет за собой загрузку библиотек и
+# первичное размещение буферов, и без прогрева первый же кадр уехал бы
+# за бюджет в 100 мс. Здесь это делается на синтетическом кадре до старта ноды.
+if [ "${FLYGUARD_WARMUP:-1}" = "1" ]; then
+ python3 - <<'PY' || echo "прогрев пропущен (не критично)"
+import numpy as np
+from scipy import ndimage
+from scipy.sparse.csgraph import connected_components
+from scipy.sparse import coo_matrix
+
+a = np.random.rand(128, 1200).astype(np.float32)
+ndimage.uniform_filter(a, size=(7, 29), mode="nearest")
+g = coo_matrix((np.ones(3, np.int8), ([0, 1, 2], [1, 2, 0])), shape=(4, 4))
+connected_components(g, directed=False)
+np.linalg.lstsq(np.random.rand(64, 3), np.random.rand(64), rcond=None)
+print("прогрев выполнен")
+PY
+fi
+
+exec "$@"
diff --git a/docker/fastdds_large.xml b/docker/fastdds_large.xml
new file mode 100644
index 0000000..0821809
--- /dev/null
+++ b/docker/fastdds_large.xml
@@ -0,0 +1,77 @@
+
+
+
+
+
+
+
+ shm_large
+ SHM
+ 134217728
+ 4194304
+ 8192
+ 2000
+
+
+
+ udp_large
+ UDPv4
+ 67108864
+ 67108864
+ 65000
+
+
+
+
+
+
+ shm_large
+ udp_large
+
+ false
+
+
+
+
+
+
+
+ ASYNCHRONOUS
+
+
+ RELIABLE
+
+
+ PREALLOCATED_WITH_REALLOC
+
+
+
+
+
+ RELIABLE
+
+
+ PREALLOCATED_WITH_REALLOC
+
+
+
+
diff --git a/docker/fastdds_udp.xml b/docker/fastdds_udp.xml
new file mode 100644
index 0000000..5bc888a
--- /dev/null
+++ b/docker/fastdds_udp.xml
@@ -0,0 +1,57 @@
+
+
+
+
+
+
+
+ udp_large
+ UDPv4
+ 67108864
+ 67108864
+ 65000
+
+
+
+
+
+
+ udp_large
+
+ false
+
+
+
+
+
+
+ ASYNCHRONOUS
+
+
+ RELIABLE
+
+
+ PREALLOCATED_WITH_REALLOC
+
+
+
+
+
+ RELIABLE
+
+
+ PREALLOCATED_WITH_REALLOC
+
+
+
+
diff --git a/docker/fetch_wheels.py b/docker/fetch_wheels.py
new file mode 100644
index 0000000..9a476e7
--- /dev/null
+++ b/docker/fetch_wheels.py
@@ -0,0 +1,59 @@
+"""Скачать колёса PyTorch с CUDA для сборки образа без сети (в docker/wheels).
+
+Нужно, когда у машины, где собирается образ, нет выхода в интернет (или он
+сломан, как у WSL за VPN), а у соседней машины есть. Скачивать можно хоть на
+Windows: колёса берутся под Linux x86_64 и Python 3.10 — ровно под образ.
+
+Тонкость: `pip download --platform` не пересчитывает маркеры окружения и на
+Windows молча пропускает зависимости «только для Linux» — все библиотеки CUDA
+и triton. Поэтому их список берётся из метаданных самого колеса torch и
+докачивается явно, с точными версиями.
+
+ python docker/fetch_wheels.py
+"""
+from __future__ import annotations
+
+import re
+import subprocess
+import sys
+import zipfile
+from pathlib import Path
+
+TORCH = "torch==2.9.1"
+INDEX = "https://download.pytorch.org/whl/cu128"
+OUT = Path(__file__).resolve().parent / "wheels"
+TARGET = ["--platform", "manylinux_2_28_x86_64", "--platform", "manylinux_2_27_x86_64",
+ "--platform", "manylinux_2_17_x86_64", "--platform", "manylinux2014_x86_64",
+ "--python-version", "3.10", "--implementation", "cp", "--abi", "cp310",
+ "--only-binary=:all:"]
+
+
+def pip_download(*args: str) -> None:
+ cmd = [sys.executable, "-m", "pip", "download", "--index-url", INDEX,
+ "--extra-index-url", "https://pypi.org/simple", "-d", str(OUT), *args]
+ subprocess.run(cmd, check=True)
+
+
+def main() -> None:
+ OUT.mkdir(exist_ok=True)
+ pip_download(TORCH, *TARGET)
+ wheel = next(OUT.glob("torch-" + TORCH.split("==")[1] + "*.whl"))
+ with zipfile.ZipFile(wheel) as z:
+ meta = next(n for n in z.namelist() if n.endswith(".dist-info/METADATA"))
+ lines = z.read(meta).decode("utf-8").splitlines()
+ linux = []
+ for line in lines:
+ m = re.match(r"Requires-Dist: ([A-Za-z0-9_.\-]+==[^;\s]+); platform_system == \"Linux\"$",
+ line)
+ if m:
+ linux.append(m.group(1))
+ print(f"зависимости только для Linux: {len(linux)}")
+ pip_download("--no-deps", *TARGET, *linux)
+ subprocess.run([sys.executable, "-m", "pip", "download", "pip", "--only-binary=:all:",
+ "--no-deps", "-d", str(OUT)], check=True)
+ size = sum(p.stat().st_size for p in OUT.glob("*.whl")) / 2**30
+ print(f"готово: {len(list(OUT.glob('*.whl')))} колёс, {size:.1f} ГБ в {OUT}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/docker/flyguard-keys.sh b/docker/flyguard-keys.sh
new file mode 100644
index 0000000..b07ab4f
--- /dev/null
+++ b/docker/flyguard-keys.sh
@@ -0,0 +1,10 @@
+#!/usr/bin/env bash
+# Пульт проигрывания для уже запущенного контейнера (из второго терминала):
+#
+# docker exec -it flyguard_demo flyguard-keys /data/<запись>
+#
+# `docker exec` не проходит через точку входа, поэтому окружение ROS
+# поднимается здесь. Клавиши — в flyguard/player_keys.py.
+source /opt/ros/humble/setup.bash
+source /opt/flyguard/ros2_ws/install/setup.bash
+exec ros2 run flyguard player_keys "$@"
diff --git a/docker/gl_probe.py b/docker/gl_probe.py
new file mode 100644
index 0000000..064da7f
--- /dev/null
+++ b/docker/gl_probe.py
@@ -0,0 +1,51 @@
+"""Чем рисует OpenGL в контейнере: видеокарта или программный llvmpipe.
+
+ docker run --rm -e DISPLAY=:0 -v /tmp/.X11-unix:/tmp/.X11-unix \
+ -v "$PWD/docker:/smoke:ro" flyguard python3 /smoke/gl_probe.py
+
+Окно не открывается: контекст создаётся на pbuffer 1×1 через EGL.
+"""
+import ctypes
+import sys
+
+EGL_OPENGL_API = 0x30A2
+EGL_SURFACE_TYPE, EGL_PBUFFER_BIT = 0x3033, 0x0001
+EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT = 0x3040, 0x0008
+EGL_WIDTH, EGL_HEIGHT, EGL_NONE = 0x3057, 0x3056, 0x3038
+GL_VENDOR, GL_RENDERER, GL_VERSION = 0x1F00, 0x1F01, 0x1F02
+
+egl = ctypes.CDLL("libEGL.so.1")
+gl = ctypes.CDLL("libGL.so.1")
+egl.eglGetDisplay.restype = ctypes.c_void_p
+egl.eglGetDisplay.argtypes = [ctypes.c_void_p]
+egl.eglInitialize.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p]
+egl.eglChooseConfig.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p,
+ ctypes.c_int, ctypes.c_void_p]
+egl.eglCreatePbufferSurface.restype = ctypes.c_void_p
+egl.eglCreatePbufferSurface.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p]
+egl.eglCreateContext.restype = ctypes.c_void_p
+egl.eglCreateContext.argtypes = [ctypes.c_void_p, ctypes.c_void_p, ctypes.c_void_p,
+ ctypes.c_void_p]
+egl.eglMakeCurrent.argtypes = [ctypes.c_void_p] * 4
+gl.glGetString.restype = ctypes.c_char_p
+
+dpy = egl.eglGetDisplay(None)
+if not dpy or not egl.eglInitialize(dpy, None, None):
+ sys.exit("EGL не инициализировался (нет DISPLAY?)")
+egl.eglBindAPI(EGL_OPENGL_API)
+attrs = (ctypes.c_int * 5)(EGL_SURFACE_TYPE, EGL_PBUFFER_BIT,
+ EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT, EGL_NONE)
+cfg, n = ctypes.c_void_p(), ctypes.c_int()
+if not egl.eglChooseConfig(dpy, attrs, ctypes.byref(cfg), 1, ctypes.byref(n)) or n.value < 1:
+ sys.exit("нет подходящей конфигурации EGL")
+surf = egl.eglCreatePbufferSurface(dpy, cfg, (ctypes.c_int * 5)(EGL_WIDTH, 1, EGL_HEIGHT, 1,
+ EGL_NONE))
+ctx = egl.eglCreateContext(dpy, cfg, None, None)
+if not ctx or not egl.eglMakeCurrent(dpy, surf, surf, ctx):
+ sys.exit("контекст OpenGL не создан")
+renderer = gl.glGetString(GL_RENDERER).decode()
+print("vendor: ", gl.glGetString(GL_VENDOR).decode())
+print("renderer:", renderer)
+print("version: ", gl.glGetString(GL_VERSION).decode())
+print("ПРОГРАММНО (процессор)" if "llvmpipe" in renderer or "softpipe" in renderer
+ else "ВИДЕОКАРТА")
diff --git a/docker/jury_cpu_test.sh b/docker/jury_cpu_test.sh
new file mode 100644
index 0000000..4ae3757
--- /dev/null
+++ b/docker/jury_cpu_test.sh
@@ -0,0 +1,41 @@
+#!/usr/bin/env bash
+# Узел на урезанном процессоре — имитация стенда жюри (i7-9700E).
+#
+# bash docker/jury_cpu_test.sh /root/bags/doubleT_obstacle # доля 0.61
+# bash docker/jury_cpu_test.sh /root/bags/cloud_with_fake_obj 0.5
+#
+# Запускается на хосте (в WSL), не в контейнере. Узел идёт в отдельном
+# контейнере с квотой процессора, а проигрыватель и подсчёт — во втором, без
+# квоты: на стенде у проигрывателя свои ядра, и делить с ним квоту узла было бы
+# строже, чем на самом деле.
+#
+# Доля по умолчанию — отношение однопоточной производительности (PassMark
+# single thread): 2511 у i7-9700E против 4129 у Ryzen 5 7600X, на котором
+# писались замеры. Узел однопоточный (BLAS в один поток), поэтому доля одного
+# ядра и есть «ядро в 1.64 раза медленнее». Для другой машины передайте свою
+# долю. Квота нарезается периодами по 10 мс, а не по 100 мс по умолчанию, —
+# иначе узел работал бы рывками по 61 мс и стоял бы по 39 мс, что искажает
+# задержку кадра сильнее, чем настоящий медленный процессор.
+set -u
+BAG="${1:?укажите путь к бэгу на хосте}"
+SHARE="${2:-0.61}"
+here="$(cd "$(dirname "$0")" && pwd)"
+bag_dir="$(cd "$(dirname "$BAG")" && pwd)"
+bag_name="$(basename "$BAG")"
+quota=$(awk -v s="$SHARE" 'BEGIN{printf "%d", s * 10000}')
+
+docker rm -f fg_slow >/dev/null 2>&1 || true
+docker run -d --name fg_slow --network host --ipc host \
+ --cpu-period 10000 --cpu-quota "$quota" flyguard > /dev/null
+echo "узел: квота $SHARE ядра ($quota мкс из 10000)"
+
+docker run --rm --network host --ipc host -e NODE=external \
+ -v "$bag_dir:/data:ro" -v "$here:/smoke:ro" flyguard \
+ bash /smoke/demo_test.sh "/data/$bag_name" 2>&1 | grep -v "^\s*$"
+
+docker kill --signal=INT fg_slow > /dev/null 2>&1
+sleep 3
+echo
+echo "=== итог узла ==="
+docker logs fg_slow 2>&1 | grep -E "итог|ошибк|Traceback" | sed 's/.*\] //'
+docker rm -f fg_slow > /dev/null 2>&1
diff --git a/docker/setup_wsl_docker.sh b/docker/setup_wsl_docker.sh
new file mode 100644
index 0000000..deada26
--- /dev/null
+++ b/docker/setup_wsl_docker.sh
@@ -0,0 +1,64 @@
+#!/usr/bin/env bash
+# Установка Docker Engine внутрь WSL Ubuntu, без Docker Desktop.
+#
+# Зачем: Docker Desktop 4.69 не стартует, если имя пользователя Windows содержит
+# кириллицу — его Inference manager не может создать unix-сокет по пути
+# C:\Users\<кириллица>\AppData\Local\Docker\run\dockerInference. Docker Engine
+# внутри WSL обходит проблему целиком и для нашей задачи даже уместнее:
+# нужен Linux с ROS 2, а WSL и есть Linux.
+#
+# Запуск (от root внутри WSL):
+# wsl -d Ubuntu -u root -- bash /mnt/c/Games/Study/AI_Lidar/docker/setup_wsl_docker.sh
+#
+# Откат:
+# apt-get remove --purge -y docker.io containerd && apt-get autoremove -y
+set -euo pipefail
+
+# Docker Desktop оставляет в PATH заглушку `docker`, которая только печатает
+# совет включить WSL-интеграцию. Поэтому всё делается по абсолютным путям.
+DOCKER=/usr/bin/docker
+DOCKERD=/usr/bin/dockerd
+
+echo "== состояние =="
+. /etc/os-release && echo "дистрибутив: ${PRETTY_NAME}"
+echo "ядер: $(nproc), свободно на /: $(df -h / | awk 'NR==2 {print $4}')"
+
+if [ -x "$DOCKERD" ]; then
+ echo "docker уже установлен: $($DOCKER --version)"
+else
+ echo "== установка docker.io =="
+ export DEBIAN_FRONTEND=noninteractive
+ apt-get update -qq
+ apt-get install -y -qq --no-install-recommends docker.io iptables
+fi
+
+# В WSL нет systemd, демон поднимается напрямую. iptables переводится в
+# legacy-режим: nft-бэкенд в WSL обычно не работает, и docker падает на
+# настройке сети.
+update-alternatives --set iptables /usr/sbin/iptables-legacy >/dev/null 2>&1 || true
+update-alternatives --set ip6tables /usr/sbin/ip6tables-legacy >/dev/null 2>&1 || true
+
+if $DOCKER info >/dev/null 2>&1; then
+ echo "демон уже работает"
+else
+ echo "== запуск демона =="
+ mkdir -p /var/log
+ nohup "$DOCKERD" >/var/log/dockerd.log 2>&1 &
+ for i in $(seq 1 40); do
+ if $DOCKER info >/dev/null 2>&1; then
+ echo "демон поднялся за ${i} с"
+ break
+ fi
+ sleep 1
+ done
+fi
+
+if ! $DOCKER info >/dev/null 2>&1; then
+ echo "ОШИБКА: демон не поднялся, последние строки журнала:" >&2
+ tail -40 /var/log/dockerd.log >&2 || true
+ exit 1
+fi
+
+echo "== готово =="
+$DOCKER version --format 'клиент {{.Client.Version}}, сервер {{.Server.Version}} ({{.Server.Os}}/{{.Server.Arch}})'
+$DOCKER info --format 'ядер {{.NCPU}}, память {{.MemTotal}}, хранилище {{.Driver}}'
diff --git a/docker/smoke_test.sh b/docker/smoke_test.sh
new file mode 100644
index 0000000..288dbe1
--- /dev/null
+++ b/docker/smoke_test.sh
@@ -0,0 +1,69 @@
+#!/usr/bin/env bash
+# Проверка собранного образа: на месте ли пакеты, сообщения, зависимости.
+# Запускается внутри контейнера, окружение ROS уже поднято точкой входа.
+set -u
+
+echo "=== пакеты ==="
+ros2 pkg list | grep -i flyguard || echo " НЕТ пакетов flyguard"
+
+echo
+echo "=== исполняемые ==="
+ros2 pkg executables flyguard || echo " НЕТ исполняемых"
+
+echo
+echo "=== сообщение ObstacleStatus ==="
+ros2 interface show flyguard_msgs/msg/ObstacleStatus 2>&1 | grep -v '^#' | grep -v '^$' | head -12
+
+echo
+echo "=== python-зависимости ==="
+python3 - <<'PY'
+import numpy, scipy
+print("numpy", numpy.__version__, "| scipy", scipy.__version__)
+try:
+ import cv2
+ print("opencv", cv2.__version__)
+except Exception as e:
+ print("opencv НЕТ:", e)
+PY
+
+echo
+echo "=== импорт конвейера ==="
+python3 - <<'PY'
+from flyguard.pipeline import FlyGuard, Params
+from flyguard.mushroom_body import MushroomBody
+from pathlib import Path
+import glob
+fg = FlyGuard(Params())
+print("конвейер импортируется, стадий в параметрах:", len(Params().__dataclass_fields__))
+hits = glob.glob("/opt/ros/**/flyguard/artifacts/mushroom_body.npz", recursive=True) \
+ + glob.glob("/opt/flyguard/**/mushroom_body.npz", recursive=True)
+if hits:
+ mb = MushroomBody.load(hits[0])
+ print(f"память тоннеля: {hits[0]}")
+ print(f" клеток Кеньона {mb.cfg.n_kc}, активных {mb.n_active}, "
+ f"обучена на {mb.n_seen} примерах, признаков {mb.n_pn}")
+else:
+ print("память тоннеля НЕ НАЙДЕНА в образе")
+from flyguard.mbon_readout import MbonReadout
+hits = glob.glob("/opt/ros/**/flyguard/artifacts/mbon_readout.npz", recursive=True) \
+ + glob.glob("/opt/flyguard/**/mbon_readout.npz", recursive=True)
+if hits:
+ rd = MbonReadout.load(hits[0])
+ print(f"считывание MBON: {hits[0]}")
+ print(f" клеток Кеньона {rd.cfg.n_kc}, признаков {rd.n_pn}")
+else:
+ print("считывание MBON НЕ НАЙДЕНО в образе")
+PY
+
+echo
+echo "=== rosbag ==="
+if ros2 bag --help >/dev/null 2>&1; then
+ echo "ros2 bag доступен"
+ python3 -c "import rosbag2_py; print('rosbag2_py ok')" 2>&1 | tail -1
+else
+ echo "ros2 bag НЕДОСТУПЕН"
+fi
+
+echo
+echo "=== launch-файл ==="
+ros2 launch flyguard detect.launch.py --show-args 2>&1 | head -20
diff --git a/docker/wheels/.gitignore b/docker/wheels/.gitignore
new file mode 100644
index 0000000..704d307
--- /dev/null
+++ b/docker/wheels/.gitignore
@@ -0,0 +1 @@
+*.whl
diff --git a/docker/wheels/README.md b/docker/wheels/README.md
new file mode 100644
index 0000000..22dda43
--- /dev/null
+++ b/docker/wheels/README.md
@@ -0,0 +1,28 @@
+# Колёса для сборки без сети
+
+Если там, где собирается образ, нет доступа к download.pytorch.org, колёса PyTorch
+скачиваются на соседней машине и отдаются сборке по HTTP — аргумент `TORCH_WHEELS`
+в docker/Dockerfile. Без аргумента сборка берёт PyTorch из индекса, как обычно.
+
+1. Скачать на машине с сетью — хоть на Windows, колёса берутся под Linux:
+
+ python docker/fetch_wheels.py
+
+ Обычного `pip download --platform ...` мало: на Windows он молча пропускает
+ зависимости «только для Linux» — все библиотеки CUDA и triton. Скрипт берёт их
+ список из метаданных колеса torch и докачивает явно, заодно кладёт колесо pip
+ (в базовом образе его нет).
+
+2. Отдать папку любым HTTP-сервером и собрать образ с сетью хоста:
+
+ python3 -m http.server 8765 --bind 127.0.0.1 --directory docker/wheels &
+ docker build --network host --build-arg TORCH_WHEELS=http://127.0.0.1:8765 \
+ -t flyguard -f docker/Dockerfile .
+
+ `--network host` нужен, чтобы сборка видела сервер на 127.0.0.1. Базовый образ
+ и пакеты apt сборка всё равно берёт из сети (или из своего кэша, если эти слои
+ уже собирались) — колёса снимают только самую тяжёлую часть, 3.8 ГБ.
+
+Колёса в контекст сборки не копируются (`.dockerignore`): это ещё 4 ГБ копий на
+диске. В репозиторий они тоже не кладутся (.gitignore) — около 4 ГБ, 26 файлов.
+После сборки папку можно очистить.
diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md
new file mode 100644
index 0000000..c2f1eb4
--- /dev/null
+++ b/docs/ARCHITECTURE.md
@@ -0,0 +1,178 @@
+# Архитектура решения
+
+Цепочка, которую просит ТЗ: **ROS 2 bag → обработка облака → алгоритм обнаружения →
+результат детекции → визуализация.**
+
+---
+
+## 1. Общая схема
+
+```
+ ros2 bag play / реальный лидар
+ │
+ sensor_msgs/PointCloud2
+ 0.3–0.9 млн точек, 10 Гц
+ │
+ ┌──────────────────────────▼──────────────────────────────┐
+ │ flyguard/node.py — ROS 2-нода │
+ │ подписка (BEST_EFFORT) → очередь на 1 кадр │
+ │ обработка в отдельном потоке, старые кадры отброшены │
+ └──────────────────────────┬──────────────────────────────┘
+ │ flyguard.cdr.PointCloud2 (без копирования)
+ ┌──────────────────────────▼──────────────────────────────┐
+ │ flyguard/pipeline.py — конвейер, хранит состояние │
+ └──────────────────────────┬──────────────────────────────┘
+ │
+ retina.py RETINA облако → дальностный образ 128 × N
+ │ выпрямление скоса каналов, слияние эх
+ ▼
+ geometry.py HALTERES плоскость рельсов: крен, тангаж, высота
+ │ ось пути: дуга u(d) = c₁d + c₂d²
+ ▼
+ lamina.py LAMINA диспаритет 1/R → ON/OFF, центр-окружение ×3
+ │
+ ├────────────────► medulla.py MEDULLA / LOBULA PLATE
+ │ T4/T5 → LPTC: скорость без одометрии
+ │ LPLC2: надвигание
+ ▼
+ lobula.py LOBULA связность с учётом глубины → кандидаты
+ │ разрез по контрасту: фигура из компоненты,
+ │ растёкшейся вдоль стены
+ │ признаки: габариты, целостность, тень, опора
+ ▼
+ mushroom_body MUSHROOM BODY PN→KC (случайно, 6 входов) → APL → MBON
+ │ новизна: 1 — незнакомо, 0 — штатная обстановка
+ │ (привыкание внутри проезда сделано и выключено: п. 10
+ │ EXPERIMENTS — избирательности у механизма нет)
+ ▼
+ fan_body.py FAN-SHAPED BODY сетка в координатах пути: опора для кандидата
+ │ там, где контраст структурно равен нулю
+ ▼
+ central_complex CENTRAL COMPLEX накопление улик в координатах пути, треки
+ │
+ ▼
+ descending.py DESCENDING два порога с гистерезисом → решение
+ │
+ ▼
+ ┌──────────────────────────────────────────────────────────┐
+ │ /flyguard/obstacle ObstacleStatus — программный выход │
+ │ /flyguard/detected Bool — бинарный статус │
+ │ /flyguard/distance Float32 — расстояние, м │
+ │ /flyguard/markers MarkerArray — рамки для RViz2 │
+ │ /flyguard/brain Image — схема мозга мухи │
+ │ /flyguard/diagnostics DiagnosticArray — задержки, скорость│
+ └──────────────────────────────────────────────────────────┘
+```
+
+---
+
+## 2. Состояние между кадрами
+
+Конвейер не обрабатывает кадры независимо. Между вызовами он хранит:
+
+| Что | Где | Зачем |
+|---|---|---|
+| решётка лучей | `FlyGuard.layout` | калибруется по первым 12 кадрам, дальше не меняется |
+| плоскость пути | `FlyGuard.plane` | сглаживание по кадрам, устойчивость к качке |
+| ось пути | `FlyGuard.corridor` | сглаживание и ограничение скорости изменения |
+| профиль и точки | `EgoMotionEstimator` | сопоставление с предыдущим кадром → скорость |
+| задержанный сигнал | `EmdBank` | вторая половина коррелятора T4/T5 |
+| сетка пути | `FanBody` | накопление лучей в координатах мира |
+| треки | `CentralComplex` | накопление улик в координатах пути |
+| гистерезис | `DescendingNeurons` | защёлка тревоги |
+
+Поэтому один экземпляр `FlyGuard` обслуживает один поток данных. Для офлайн-экспериментов
+с несколькими сценариями одновременно создаётся несколько экземпляров.
+
+---
+
+## 3. Потоки и реальное время
+
+Нода разделена на два потока:
+
+* **поток ROS** принимает облака и кладёт в слот на один кадр; если предыдущий ещё не
+ обработан, он **отбрасывается** и счётчик `dropped_frames` растёт;
+* **рабочий поток** берёт последний кадр и гоняет конвейер.
+
+Так система реального времени отвечает на текущую обстановку, а не доедает накопившееся
+прошлое. Число отброшенных кадров публикуется в диагностике: если оно растёт, значит
+машина не тянет, и это видно сразу, а не проявляется скрытой задержкой.
+
+Замер по стадиям ведётся всегда и публикуется в `/flyguard/diagnostics`, поэтому
+профилировать решение можно прямо на стенде, не пересобирая его.
+
+---
+
+## 4. Разделение на пакеты
+
+```
+ros2_ws/src/
+├── flyguard_msgs/ ament_cmake — только сообщения
+│ └── msg/ObstacleStatus.msg, msg/DetectedObject.msg
+└── flyguard/ ament_python — конвейер и нода
+ ├── flyguard/
+ │ ├── cdr.py разбор PointCloud2 без ROS (офлайн-режим)
+ │ ├── bag.py чтение rosbag2 sqlite3 без ROS
+ │ ├── ros_conv.py sensor_msgs → внутреннее представление
+ │ ├── retina.py решётка лучей, дальностный образ
+ │ ├── geometry.py плоскость пути, ось, координаты (d, u, h)
+ │ ├── lamina.py ON/OFF, центр-окружение
+ │ ├── medulla.py T4/T5, LPTC, LPLC2, оценка движения
+ │ ├── lobula.py кандидаты, связность с учётом глубины
+ │ ├── mushroom_body.py новизна
+ │ ├── central_complex.py треки
+ │ ├── descending.py решение
+ │ ├── synth.py синтетические препятствия (для полигона)
+ │ ├── brain_view.py схема мозга мухи
+ │ ├── pipeline.py сборка
+ │ ├── node.py ROS 2-нода
+ │ └── data/pandar128_channels.csv поканальная таблица из руководства
+ ├── launch/detect.launch.py
+ ├── config/flyguard.yaml, config/flyguard.rviz
+ └── test/test_pipeline.py
+```
+
+Ключевое решение: **ядро не зависит от ROS**. `rclpy` импортируется только в `node.py`.
+Благодаря этому весь конвейер запускается офлайн прямо по `.db3`, что дало возможность
+отлаживать и мерить качество на Windows без ROS и быстро гонять полигон в несколько
+параллельных сценариев.
+
+---
+
+## 5. Артефакты
+
+| Файл | Что это | Как получен |
+|---|---|---|
+| `artifacts/mushroom_body.npz` | память тоннеля, без учителя | `tools/train_mushroom_body.py` на пустых проездах |
+| `artifacts/mbon_readout.npz` | обученное считывание MBON | `tools/train_mbon.py` на размеченной вставками выборке |
+| `artifacts/mbon_folds/` | по модели на складку, для честной проверки | он же, ключ `--save-folds` |
+| `data/cache/training_set.npz` | размеченная выборка кандидатов | `tools/make_training_set.py` |
+| `artifacts/generalisation.json` | leave-one-bag-out | `tools/evaluate.py` |
+| `artifacts/benchmark.json` | кривые дальности | `tools/make_benchmark.py` |
+| `flyguard/data/pandar128_channels.csv` | 128 каналов лидара | `tools/extract_channel_table.py` из руководства |
+
+Память тоннеля копируется в Docker-образ и подхватывается launch-файлом автоматически;
+путь переопределяется параметром `memory_path`.
+
+---
+
+## 6. Потоки данных в цифрах
+
+Для кадра 128 × 3600 × 2 эха (полный круговой скан, 921 600 точек):
+
+| Стадия | Объём на входе | Время, мс |
+|---|---|---|
+| приём и разбор сообщения | 24 МБ | ~2 (без копирования) |
+| retina (оконная проекция) | только нужный сектор | 6–9 |
+| стабилизация | ~150 тыс. точек | 3–4 |
+| ось пути | 30 срезов | 4–5 |
+| ламина | 128 × 600 × 3 масштаба | 6–7 |
+| оценка движения | 6000 точек × ~20 проб | 5–8 |
+| лобула | связность по маске | 3–5 |
+| грибовидное тело | единицы кандидатов | 1–2 |
+| центральный комплекс и решение | десятки треков | <0.3 |
+
+Оконная проекция — важная оптимизация: при секторе обработки ±30° тяжёлая арифметика
+выполняется только над теми сырыми столбцами, которые в него попадут с учётом скоса
+каналов. На круговом скане это сократило стадию ретины с 29 до 7 мс без изменения
+результата (проверено побитовым сравнением).
diff --git a/docs/CONNECTOME.md b/docs/CONNECTOME.md
index 00ad01a..97db3a6 100644
--- a/docs/CONNECTOME.md
+++ b/docs/CONNECTOME.md
@@ -147,6 +147,36 @@ T4 получает вход из ON-пути, T5 — из OFF-пути; каж
---
+## 3.5. Атлас: те же клетки, видимые глазом
+
+Проверить, что архитектура действительно собрана из заявленных схем, можно не на
+словах. `tools/build_brain_atlas.py` берёт публичные выгрузки Codex (FAFB v783) и
+сводит их в атлас: положение сомы каждого нейрона и номер стадии FlyGuard, которой
+он соответствует. Привязка идёт по **имени типа клетки**, а не по догадке, и все
+типы находятся поимённо:
+
+| Тип | В коннектоме | Где используется |
+|---|---|---|
+| LC11 | 127 | `lobula.py` — кандидаты как компактные группы лучей |
+| LPLC2 | 210 | `medulla.py` — надвигание как дивергенция потока |
+| HS (HSE/HSN/HSS) | 6 | `medulla.py` — широкопольный поток |
+| VS1…VS8 | 16 | там же, вертикальный канал |
+| T4a–d / T5a–d | 6243 / 6002 | `medulla.py` — коррелятор Хассенштайна–Райхардта |
+| L1 / L2 | 3503 | `lamina.py` — каналы ON и OFF |
+| Клетки Кеньона | 5177 | `mushroom_body.py` — разрежённый код |
+| MBON | 96 | там же, выход памяти |
+| APL | 2 | там же, глобальное торможение |
+| EPG | 47 | `central_complex.py` — кольцевой аттрактор |
+| DNp01 | 2 | `descending.py` — гигантское волокно |
+
+Совпадение с литературой полное: LC11 около 110–130 клеток, HS по три на сторону,
+VS по восемь, APL и гигантское волокно — по одной на полушарие.
+
+Вид `brain_style: cloud` рисует эти 139 255 нейронов на их анатомических местах и
+подсвечивает активностью соответствующей стадии. Симуляции по-прежнему нет:
+коннектом даёт анатомию, конвейер даёт активность, вид накладывает одно на другое
+за 8 мс на кадр.
+
## 4. Источники
* Dorkenwald S. и др. **Neuronal wiring diagram of an adult brain.** Nature, 2024 —
diff --git a/docs/EXPERIMENTS.md b/docs/EXPERIMENTS.md
index ef7780b..860e77f 100644
--- a/docs/EXPERIMENTS.md
+++ b/docs/EXPERIMENTS.md
@@ -252,6 +252,86 @@ numba являются ламина и оценка движения — вме
проекция сократила стадию с **29 до 7 мс**, причём результат совпадает с полной
проекцией **побитово** — проверено сравнением массивов.
+### 7.1. Замеры в собранном контейнере
+
+Всё выше измерено офлайн. Ниже — то же, но внутри Docker-образа, через настоящий
+ROS 2 и `ros2 bag play`. Счётчики берутся из самой ноды и публикуются в
+`/flyguard/diagnostics`: внешний подписчик сам теряет сообщения и занижает оценку.
+
+| Бэг | Кадр | Всего в бэге | Принято | На калибровку | Обработано | Отброшено | Медиана | p95 |
+|---|---|---|---|---|---|---|---|---|
+| `roundT_doubleT` | 8 МБ | 252 | **252** | 11 | 241 | **0** | 43.0 мс | 54.4 мс |
+| `doubleT_obstacle` | 24 МБ | 201 | **201** | 11 | 190 | **0** | 31.9 мс | 41.2 мс |
+
+Принимается **всё**. Одиннадцать кадров уходят на восстановление решётки лучей по самим
+данным — это цена того, что в решении не захардкожена геометрия сенсора.
+
+Нода обрабатывает **всё, что до неё доходит**, на полных 10 Гц и с трёхкратным
+запасом по бюджету. Реальный объект в контейнере обнаруживается в **98.9 %** кадров —
+ровно столько же, сколько офлайн. Ложные тревоги на `roundT_doubleT` в контейнере
+**1.2 %** кадров (было 40.6 % до исправления контекста, см. 5.1).
+
+### 7.2. Где на самом деле теряются кадры
+
+Сначала до ноды на `doubleT_obstacle` доходило 106 сообщений из 201. Проверка показала,
+что алгоритм тут ни при чём: **подписчик, который вообще ничего не делает**, получал
+ровно столько же (99 из 201). Потери были в транспорте, и у них нашлись три причины.
+
+**1. Умолчания Fast DDS рассчитаны на сотни килобайт.** Сегмент разделяемой памяти —
+512 КБ, буферы сокетов ОС — 208 КБ, при кадре в 24 МБ. Кадр не помещается, разделяемая
+память не используется, всё идёт по UDP десятками тысяч фрагментов. Профиль
+`docker/fastdds_large.xml` поднимает сегмент до 128 МБ, буферы до 64 МБ и ставит
+разделяемую память первой; он прописан в образ через `FASTRTPS_DEFAULT_PROFILES_FILE`.
+
+**2. `/dev/shm` в контейнере по умолчанию 64 МБ.** Сегмент туда не влезает, Fast DDS
+молча откатывается на UDP. Нужен `--shm-size=1g`; точка входа предупреждает, если
+меньше. Сегмент заводит каждый участник DDS, а их в сеансе обычно три (нода,
+проигрыватель, наблюдатель) — поэтому 128 МБ, а не 512: при 512 третий участник
+не помещался и получал `Unable to Register SHM Transport`.
+
+**3. Проигрыватель набивает очередь чтения до старта.** `ros2 bag play` по умолчанию
+читает вперёд 1000 сообщений; при кадре в 24 МБ это 24 ГБ. Пока он читает, часы записи
+уже идут, и первые секунды кадров «просрочиваются» и не публикуются вовсе. Помогает
+`--read-ahead-queue-size 10`.
+
+Итог по 24-мегабайтному бэгу (201 кадр всего), приёмник без обработки и полный конвейер:
+
+| Конфигурация | Пустой приёмник | Полный конвейер |
+|---|---|---|
+| умолчания | 99 | 106 |
+| + профиль DDS и `--shm-size=1g` | **201** | 125 |
+| + `--read-ahead-queue-size 10` | — | 181 |
+| + быстрая калибровка решётки | — | **201** |
+
+**4. Калибровка решётки блокировала колбэк на 2.8 секунды.** Оставшиеся два десятка
+кадров терялись не в транспорте: восстановление решётки по 12 кадрам считало медиану
+по стопке из 12 массивов 128 × 7200 — 1.4 с только на два `nanmedian`. Пока колбэк
+занят, очередь на 10 кадров переполняется.
+
+Направление луча задано сенсором и в каждом кадре одно и то же; кадры нужны лишь
+чтобы закрыть лучи без эха. Медиана заменена на первое конечное значение, счёт —
+на float32: **2806 мс → 373 мс**. Сверка с паспортом после замены не изменилась ни в
+одном знаке (медиана ошибки 0.0645° по элевации, 0.0435° по сдвигу — как и раньше).
+Очередь поднята до 20 кадров как страховка.
+
+Заодно исправлен счётчик: `frames_received` считался как «обработано + отброшено» и
+не включал кадры калибровки, из-за чего в отчёте выглядела потеря там, где её не было.
+
+Отдельно применено в самой ноде:
+
+* подписка по умолчанию **RELIABLE** — при BEST_EFFORT потеря одного UDP-фрагмента
+ отбрасывает всё 24-мегабайтное сообщение целиком;
+* кадр берётся **сырыми байтами CDR** (`raw_subscription`), минуя сборку
+ Python-объекта `sensor_msgs` — на 24 МБ она стоит дороже всей нашей обработки;
+* обработка идёт **в колбэке, а не в отдельном потоке**: поток боролся за GIL
+ с приёмом и ронял выработку с 10 до 2 Гц при неизменном такте в 32 мс.
+
+Почему не переписали конвейер на компилируемый язык: он тратит 32 мс из 100 и
+простаивает две трети такта, а терялись кадры **до** его вызова. Ускорение обработки
+не вернуло бы ни одного кадра; настройка транспорта вернула 82.
+
+---
+
## 7.3. Почему далёкий предмет теряется — и что нужно для 200 м
ТЗ просит 300 м как «отлично» и 200 м как «очень хорошо». Разберём честно, чего
@@ -339,7 +419,7 @@ numba являются ламина и оценка движения — вме
| Кадров с тревогой | **9.4 %** | 17.2 % |
| Посторонних тревог на кадр (полигон) | **0.055** | 0.171 |
| Реальный объект на 55 м | 98.9 % | 98.9 % |
-| Задержка, медиана | 32 мс | 33 мс |
+| Задержка, медиана в контейнере | 32 мс | 33 мс |
**Включено по умолчанию.** Размен здесь принципиально лучше, чем у разделения фигуры
и фона (п. 9.4): там было вчетверо больше ложных за +29 % дальности, здесь — в 1.75
diff --git a/docs/figures/curve_gauge.png b/docs/figures/curve_gauge.png
new file mode 100644
index 0000000..4602a6c
Binary files /dev/null and b/docs/figures/curve_gauge.png differ
diff --git a/docs/figures/doubleT_mount_shadow.png b/docs/figures/doubleT_mount_shadow.png
new file mode 100644
index 0000000..078a9b7
Binary files /dev/null and b/docs/figures/doubleT_mount_shadow.png differ
diff --git a/flyguard/brain_atlas.py b/flyguard/brain_atlas.py
new file mode 100644
index 0000000..5337c41
--- /dev/null
+++ b/flyguard/brain_atlas.py
@@ -0,0 +1,364 @@
+"""Мозг как облако из 139 255 нейронов, подсвеченное живой активностью.
+
+Отличие от `brain_view` — не в красоте, а в том, чем нарисовано. Там схема,
+нарисованная руками; здесь каждая точка — **настоящий нейрон из коннектома**
+FlyWire FAFB v783, стоящий на своём анатомическом месте, а его яркость берётся
+от той стадии FlyGuard, которой он соответствует.
+
+Симуляции нет и не нужно. Мембранные потенциалы 139 тысяч клеток никто не
+интегрирует: коннектом даёт анатомию и принадлежность клеток, конвейер даёт
+активность по стадиям, а здесь одно накладывается на другое. Стоимость кадра —
+взвешенная сумма десяти заранее посчитанных карт плотности.
+
+Привязка держится на именах типов клеток, а не на догадках. FlyGuard с самого
+начала собран из конкретных схем, и все они в выгрузке есть поимённо:
+
+ LC11 127 нейронов детектор мелкого объекта → lobula.py
+ LPLC2 210 детектор надвигания → medulla.py
+ HS/VS 22 широкопольный поток → medulla.py
+ T4/T5 12245 элементарные детекторы движения
+ KC 5177 клетки Кеньона, разрежённый код → mushroom_body.py
+ MBON 96 выход памяти, новизна
+ APL 2 глобальное торможение
+ DNp01 2 гигантское волокно, экстренное решение → descending.py
+
+Атлас собирается `tools/build_brain_atlas.py` и лежит в `data/brain_atlas.npz`
+(605 КБ). Данные FlyWire — CC-BY 4.0, Dorkenwald et al. и Schlegel et al.,
+Nature 2024.
+"""
+from __future__ import annotations
+
+from collections import deque
+from pathlib import Path
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError:
+ cv2 = None
+
+from .brain_view import BG, INK, _colour, put_text, text_size
+
+ATLAS = Path(__file__).with_name("data") / "brain_atlas.npz"
+
+# Какая стадия конвейера освещает какую группу нейронов. Ключи справа — из
+# `BrainView.activity`; так обе картинки всегда показывают одно и то же.
+STAGE_SOURCE: dict[str, tuple[str, ...]] = {
+ "retina": ("retina",),
+ "lamina": ("lamina",),
+ "medulla": ("medulla",),
+ "lptc": ("lobula_plate",),
+ "looming": ("looming",),
+ "lobula": ("lobula",),
+ "mushroom": ("mb_lobes", "mb_calyx"),
+ "central": ("eb", "pb", "fb"),
+ "descending": ("dn",),
+ "other": (),
+}
+OTHER_BASE = 0.045 # остальной мозг: виден, но не отвлекает
+TRACE_N = 100 # длина ритмограммы: 10 с при 10 Гц
+
+RU = {
+ "retina": "сетчатка R1–R8",
+ "lamina": "ламина L1 / L2",
+ "medulla": "медулла T4 / T5",
+ "lptc": "лоб. пластинка HS / VS",
+ "looming": "LPLC2 надвигание",
+ "lobula": "лобула LC11",
+ "mushroom": "грибовидное тело KC → MBON",
+ "central": "центральный комплекс",
+ "descending": "нисходящие DNp01",
+}
+
+
+class NeuronCloud:
+ """Облако нейронов в анатомических координатах, яркость — активность стадии."""
+
+ def __init__(self, labels: bool = True, gain: float = 1.0, scale: int = 1):
+ self.enabled = cv2 is not None and ATLAS.exists()
+ self.labels = labels
+ self.gain = gain
+ # Масштаб отрисовки: 1 — для узла (1180 × 620), 2–3 — для экрана и
+ # видео в 2K/4K. Сомы берутся из атласа своего размера
+ # (brain_atlas_x2.npz …) — это настоящие координаты, а не растянутая
+ # картинка; шрифты, линии и отступы растут вместе с масштабом.
+ self.s = max(1, int(scale))
+ self._smooth: dict[str, float] = {}
+ self._trace: deque = deque(maxlen=TRACE_N)
+ if not self.enabled:
+ return
+
+ path = ATLAS.with_name(f"brain_atlas_x{self.s}.npz") if self.s > 1 else ATLAS
+ d = np.load(path if path.exists() else ATLAS, allow_pickle=False)
+ self.h, self.w = (int(v) for v in d["shape"])
+ stages = [str(s) for s in d["stages"]]
+ px, py = d["px"].astype(np.int64), d["py"].astype(np.int64)
+ stage, side, depth = d["stage"], d["side"], d["depth"]
+ if not path.exists(): # атласа нужного размера нет — растянуть обычный
+ px, py, self.h, self.w = px * self.s, py * self.s, self.h * self.s, self.w * self.s
+
+ ok = (px >= 0) & (px < self.w) & (py >= 0) & (py < self.h)
+ flat = py * self.w + px
+ # Глубина: дальние сомы тусклее, иначе фронтальная проекция выглядит плоской
+ shade = (0.55 + 0.45 * (1.0 - depth / 255.0)).astype(np.float32)
+
+ # Сомы занимают шестую часть холста; считать по всему полотну незачем.
+ # Карты сразу строятся по занятым пикселям: это убирает из каждого
+ # кадра 80 % работы, а в 4K — ещё и сотни мегабайт промежуточных карт.
+ self.support = np.unique(flat[ok])
+ at = np.searchsorted(self.support, flat)
+
+ # Две карты на стадию — левое и правое полушарие: так видно, с какой
+ # стороны пришёл объект, ровно как у мухи с её двумя оптическими долями.
+ self.keys: list[str] = []
+ maps = []
+ for i, name in enumerate(stages):
+ for sd, tag in ((0, "L"), (1, "R"), (2, "C")):
+ m = ok & (stage == i) & (side == sd)
+ if m.sum() == 0:
+ continue
+ self.keys.append(f"{name}|{tag}")
+ maps.append(np.bincount(at[m], weights=shade[m],
+ minlength=self.support.size).astype(np.float32))
+ self.maps = np.ascontiguousarray(np.stack(maps)) # (K, занятые пиксели)
+ self.stage_of = [k.split("|")[0] for k in self.keys]
+ self.side_of = [k.split("|")[1] for k in self.keys]
+
+ # Якоря подписей — центры тяжести стадии в своём полушарии. Общий
+ # центр тяжести у всех стадий приходится на середину мозга и ничего
+ # не показывает: стадии парные.
+ self.anchor: dict[str, tuple[tuple[int, int], tuple[int, int]]] = {}
+ for i, name in enumerate(stages):
+ if name not in RU:
+ continue
+ a = []
+ for sd in (0, 1):
+ m = ok & (stage == i) & (side == sd)
+ m = m if m.sum() > 30 else (ok & (stage == i))
+ a.append((int(px[m].mean()), int(py[m].mean())))
+ self.anchor[name] = tuple(a)
+
+ self._lut = self._build_lut()
+ self._base = self._draw_base()
+
+ # ------------------------------------------------------------------ статика
+
+ @staticmethod
+ def _build_lut() -> np.ndarray:
+ """Палитра «покой → работа → тревога», та же, что у схемы."""
+ lut = np.zeros((256, 3), np.uint8)
+ for i in range(256):
+ lut[i] = _colour(i / 255.0)
+ return lut
+
+ def _draw_base(self) -> np.ndarray:
+ s = self.s
+ img = np.full((self.h, self.w, 3), BG, np.uint8)
+ put_text(img, "FlyGuard — 139 255 нейронов коннектома FlyWire, "
+ "подсвеченных активностью конвейера",
+ (26 * s, 34 * s), 0.6 * s, (205, 205, 215))
+ cv2.line(img, (26 * s, 46 * s), (self.w - 26 * s, 46 * s), (52, 52, 62), s)
+ return img
+
+ # ------------------------------------------------------------------ кадр
+
+ def weights(self, act: dict[str, float], bias: float) -> np.ndarray:
+ """Яркость каждой полукарты. `bias` < 0 — объект слева, > 0 — справа."""
+ w = np.empty(len(self.keys), np.float32)
+ for i, (stage, side) in enumerate(zip(self.stage_of, self.side_of)):
+ src = STAGE_SOURCE.get(stage, ())
+ v = max((self._smooth.get(k, 0.0) for k in src), default=0.0)
+ v = OTHER_BASE if not src else max(v, OTHER_BASE * 0.7)
+ if side in ("L", "R") and stage in ("retina", "lamina", "medulla",
+ "lptc", "looming", "lobula"):
+ # У мухи зрительный мир перекрещен: предмет справа сильнее
+ # освещает левую долю. Для демонстрации важно, что асимметрия
+ # видна и соответствует стороне объекта.
+ v *= 1.0 + 0.10 * (bias if side == "L" else -bias)
+ w[i] = np.clip(v, 0.0, 1.0)
+ return w
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ from .brain_view import BrainView
+
+ for k, v in BrainView.activity(res).items():
+ self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
+
+ # Ритмограмма: у дрозофилы ритмы мозга меряют электродом, из наших
+ # данных они не выводятся. Поэтому здесь честный след собственного
+ # состояния — сколько работает зрительный тракт, сколько памяти и
+ # когда сработало решение.
+ vis = float(np.mean([self._smooth.get(k, 0.0) for k in
+ ("lamina", "medulla", "lobula", "lobula_plate")]))
+ self._trace.append((float(vis),
+ float(self._smooth.get("dopamine", 0.0)),
+ bool(res.decision.detected),
+ float(res.total_ms),
+ float(self._smooth.get("habituation", 0.0))))
+
+ lat = [c.u for c in res.candidates] or [0.0]
+ bias = float(np.clip(np.mean(lat) / 1.6, -1.0, 1.0))
+
+ vals = self.weights(self.activity_smoothed(), bias) @ self.maps
+ img = self._base.copy()
+ if self.s == 1:
+ idx = np.clip(vals * (255.0 * self.gain), 0, 255).astype(np.uint8)
+ img.reshape(-1, 3)[self.support] = self._lut[idx]
+ # Точки одиночные и на большом холсте теряются: лёгкое размытие делает
+ # облако читаемым, не превращая его в заливку.
+ cv2.GaussianBlur(img, (3, 3), 0, dst=img)
+ else:
+ self._paint_fine(img, vals)
+
+ if self.labels:
+ self._draw_labels(img)
+ self._draw_rhythm(img)
+ self._draw_readout(img, res)
+ return img
+
+ def _paint_fine(self, img, vals) -> None:
+ """Облако в высоком разрешении: каждая сома на своём месте.
+
+ В масштабе 1 пиксель холста собирает несколько сом, и яркость — их
+ сумма. Здесь сома — отдельная точка, поэтому сумма берётся по
+ окрестности той же площади (гауссово окно, нормированное так, что
+ одиночная сома сохраняет свою яркость), а края точек смешиваются с
+ фоном по доле покрытия. Облако выглядит как в узле, только чётче.
+ """
+ sig = 0.55 * self.s
+ norm = 2.0 * np.pi * sig * sig
+ field = np.zeros((2, self.h * self.w), np.float32)
+ field[0, self.support] = vals
+ field[1, self.support] = 1.0
+ v = cv2.GaussianBlur(field[0].reshape(self.h, self.w), (0, 0), sig) * norm
+ a = np.clip(cv2.GaussianBlur(field[1].reshape(self.h, self.w), (0, 0), sig)
+ * norm, 0.0, 1.0)
+ idx = np.clip(v * (255.0 * self.gain), 0, 255).astype(np.uint8)
+ col = self._lut[idx]
+ img[:] = cv2.blendLinear(col, img, a, 1.0 - a)
+
+ def activity_smoothed(self) -> dict[str, float]:
+ return self._smooth
+
+ def _draw_labels(self, img) -> None:
+ """Подписи столбиком у ближнего края с выноской к центру тяжести стадии.
+
+ Центры тяжести половины стадий лежат в середине мозга и налезают друг
+ на друга, поэтому подписи разводятся по вертикали, а привязка остаётся
+ честной — линия ведёт ровно в центр тяжести.
+ """
+ # Зрительный путь подписывается слева, память и решение — справа:
+ # так выноски не пересекают мозг и идут в своё же полушарие.
+ s = self.s
+ left_side = ("retina", "lamina", "medulla", "lptc")
+ cols = {True: 76 * s, False: 76 * s}
+ for name in ("retina", "lamina", "medulla", "lptc",
+ "looming", "lobula", "mushroom", "central", "descending"):
+ if name not in self.anchor:
+ continue
+ left = name in left_side
+ ax, ay = self.anchor[name][0 if left else 1]
+ txt = RU[name]
+ tw, th = text_size(txt, 0.42 * s)
+ x = 26 * s if left else self.w - tw - 26 * s
+ y = cols[left]
+ cols[left] += 27 * s
+ box = img[max(y - th - 5 * s, 0):y + 5 * s, max(x - 6 * s, 0):x + tw + 6 * s]
+ if box.size:
+ box[:] = (box * 0.2).astype(np.uint8)
+ put_text(img, txt, (x, y), 0.42 * s, INK)
+ tip = (x + tw + 7 * s, y - th // 2) if left else (x - 7 * s, y - th // 2)
+ cv2.line(img, tip, (ax, ay), (82, 80, 96), s, cv2.LINE_AA)
+ cv2.circle(img, (ax, ay), 3 * s, (162, 160, 178), s, cv2.LINE_AA)
+
+ def _draw_rhythm(self, img) -> None:
+ """Ритмограмма: след активности и дофамина за последние десять секунд.
+
+ Это не ритм мозга дрозофилы — тот меряют электродом, и из лидарных
+ данных он не выводится. Это след собственного состояния системы:
+ зрительный тракт, сигнал обучения и отметки моментов решения.
+ """
+ if len(self._trace) < 3:
+ return
+ s = self.s
+ x0, y0, w, hgt = 250 * s, self.h - 96 * s, 380 * s, 56 * s
+ cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (40, 39, 48), -1)
+ cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (60, 58, 70), s)
+ put_text(img, "ритм · 10 с", (x0 + 4 * s, y0 - 6 * s), 0.37 * s, (128, 126, 140))
+
+ n = len(self._trace)
+ xs = x0 + (np.arange(n) * (w - 6 * s) / max(TRACE_N - 1, 1)).astype(int) + 3 * s
+ tr = np.array([(a, b, float(c), e) for a, b, c, _, e in self._trace],
+ np.float32)
+
+ lx = x0 + 78 * s
+ traces = [(0, (176, 196, 92), "зрение"), (1, (86, 178, 240), "дофамин")]
+ if tr[:, 3].max() > 0.0:
+ traces.append((3, (150, 120, 226), "привыкание"))
+ for col, colour, name in traces:
+ ys = (y0 + hgt - 8 * s - tr[:, col] * (hgt - 16 * s)).astype(int)
+ cv2.polylines(img, [np.stack([xs, ys], 1)], False, colour, s, cv2.LINE_AA)
+ cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), colour, 2 * s,
+ cv2.LINE_AA)
+ put_text(img, name, (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
+ lx += 15 * s + text_size(name, 0.35 * s)[0] + 16 * s
+
+ # Полоска решений внизу: где система говорила «препятствие».
+ hot = tr[:, 2] > 0.5
+ if hot.any():
+ for i in np.flatnonzero(hot):
+ cv2.line(img, (int(xs[i]), y0 + hgt - 5 * s), (int(xs[i]), y0 + hgt - 2 * s),
+ (66, 66, 248), s)
+ cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), (66, 66, 248), 2 * s)
+ put_text(img, "решение", (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
+
+ def _draw_readout(self, img, res) -> None:
+ d = res.decision
+ hab = float(getattr(res, "hab", 0.0))
+ rows = [("скорость", f"{abs(d.speed) * 3.6:.1f} км/ч"),
+ ("кандидатов", f"{len(res.candidates)}"),
+ ("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
+ ("дофамин PPL1/PAM", f"{self._smooth.get('dopamine', 0.0):.2f}")]
+ # привыкание по умолчанию выключено — строка появляется, только если оно
+ # включено и уже что-то накопило
+ if hab > 0.0:
+ rows.append(("привыкание", f"{hab:.2f}"))
+ rows.append(("обработка", f"{res.total_ms:.1f} мс"))
+ s = self.s
+ y0 = self.h - 126 * s - 23 * s * (len(rows) - 5)
+ # Столбик лежит поверх облака, и серые подписи в нём тонули в точках
+ # оптической доли — подложка, как под выносками стадий.
+ vw = max(text_size(v, 0.46 * s)[0] for _, v in rows)
+ right = min(196 * s + vw + 10 * s, 244 * s) # не заходить на ритмограмму
+ box = img[y0 - 18 * s:y0 + 23 * s * (len(rows) - 1) + 8 * s, 18 * s:right]
+ box[:] = (box * 0.25).astype(np.uint8)
+ for i, (k, v) in enumerate(rows):
+ put_text(img, k, (28 * s, y0 + i * 23 * s), 0.44 * s, (118, 116, 128))
+ put_text(img, v, (196 * s, y0 + i * 23 * s), 0.46 * s, (206, 206, 216))
+
+ if d.detected:
+ txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
+ sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
+ else f"уверенность {d.confidence:.2f}")
+ colour = (66, 66, 248) if d.emergency else (72, 196, 240)
+ else:
+ txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
+ put_text(img, txt, (self.w - 470 * s, self.h - 62 * s), 0.9 * s, colour, 2)
+ if sub:
+ put_text(img, sub, (self.w - 470 * s, self.h - 32 * s), 0.54 * s, colour)
+
+ x = self.w - 300 * s
+ for lvl, name in ((0.10, "покой"), (0.45, "работа"), (0.85, "тревога")):
+ cv2.circle(img, (x, 34 * s), 5 * s, _colour(lvl), -1, cv2.LINE_AA)
+ put_text(img, name, (x + 12 * s, 39 * s), 0.4 * s, (150, 148, 162))
+ x += 14 * s + text_size(name, 0.4 * s)[0] + 22 * s
+
+ # ------------------------------------------------------------------ ROS
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from .brain_view import BrainView
+ return BrainView.to_msg(img, stamp, frame_id)
diff --git a/flyguard/brain_hybrid.py b/flyguard/brain_hybrid.py
new file mode 100644
index 0000000..9ddfc02
--- /dev/null
+++ b/flyguard/brain_hybrid.py
@@ -0,0 +1,145 @@
+"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
+
+Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
+образ, ON-канал ламины (что ближе окружения), OFF-канал (что дальше) и
+выделенные кандидаты. Снизу — те же 139 255 нейронов коннектома, подсвеченные
+активностью соответствующих стадий.
+
+Смысл именно в паре: видно не только «мозг светится», но и от чего. Контраст
+в ламине появляется на панели — и в ту же секунду загорается полоса ламины в
+облаке; предмет попадает в габарит — вспыхивает лобула; трек набирает улику —
+разгорается центральный комплекс и уходит вниз к гигантскому волокну.
+
+Стоит около 12 мс и включается флагом, чтобы не попадать в замеры задержки.
+"""
+from __future__ import annotations
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError:
+ cv2 = None
+
+from .brain_atlas import NeuronCloud
+from .brain_view import BG, put_text
+
+PANEL_H = 132 # высота строки панелей, px
+PAD = 10
+
+
+def _norm(a: np.ndarray, lo: float, hi: float) -> np.ndarray:
+ return np.clip((a - lo) / max(hi - lo, 1e-6), 0.0, 1.0)
+
+
+class BrainHybrid:
+ """Панели решётки + облако нейронов в одном кадре."""
+
+ def __init__(self, labels: bool = True, scale: int = 1):
+ self.cloud = NeuronCloud(labels=labels, scale=scale)
+ self.s = self.cloud.s
+ self.enabled = cv2 is not None and self.cloud.enabled
+ if not self.enabled:
+ return
+ self.w = self.cloud.w
+ self.strip_h = (PANEL_H + 34) * self.s
+ self.h = self.cloud.h + self.strip_h
+
+ # ------------------------------------------------------------------ панели
+
+ def _panel(self, img: np.ndarray, title: str, hint: str, slot: int,
+ out: np.ndarray) -> None:
+ n, s = 4, self.s
+ pad, ph, top = PAD * s, PANEL_H * s, 28 * s
+ pw = (self.w - pad * (n + 1)) // n
+ x = pad + slot * (pw + pad)
+ # INTER_AREA, а не NEAREST: столбцы чередуются по эхам, и при
+ # прореживании соседями решётка превращается в шахматку.
+ small = cv2.resize(img, (pw, ph), interpolation=cv2.INTER_AREA)
+ out[top:top + ph, x:x + pw] = small
+ cv2.rectangle(out, (x, top), (x + pw, top + ph), (58, 56, 68), s)
+ from .brain_view import text_size
+ put_text(out, title, (x + 2 * s, 22 * s), 0.4 * s, (188, 188, 200))
+ if hint:
+ tw = text_size(hint, 0.36 * s)[0]
+ # подпись справа рисуется только если не налезает на заголовок
+ if x + pw - tw - 8 * s > x + 2 * s + text_size(title, 0.4 * s)[0]:
+ put_text(out, hint, (x + pw - tw - 2 * s, 22 * s), 0.36 * s, (112, 110, 124))
+
+ def _strip(self, res) -> np.ndarray:
+ out = np.full((self.strip_h, self.w, 3), BG, np.uint8)
+ tf, lam = res.tf, res.lam
+ if tf is None or lam is None:
+ put_text(out, "панели решётки доступны только с keep_debug=True",
+ (PAD * self.s, 24 * self.s), 0.44 * self.s, (140, 138, 152))
+ return out
+
+ # 1. Дальностный образ: ближнее ярче, «нет эха» — ровный тёмный фон,
+ # иначе пустые лучи красятся палитрой и спорят с данными.
+ r = np.where(tf.valid, tf.r, np.nan)
+ depth = 1.0 - _norm(np.nan_to_num(r, nan=220.0), 4.0, 140.0)
+ rng = cv2.applyColorMap((depth * 255).astype(np.uint8), cv2.COLORMAP_TURBO)
+ rng[~tf.valid] = (34, 32, 38)
+ self._panel(rng, "дальностный образ", "R1–R8", 0, out)
+
+ # 2. ON: ближе окружения — то, что торчит навстречу
+ on = _norm(lam.on, 0.0, 0.02)
+ self._panel(cv2.applyColorMap((on * 255).astype(np.uint8),
+ cv2.COLORMAP_INFERNO),
+ "ламина L1 · ON-контраст", "ближе фона", 1, out)
+
+ # 3. OFF: дальше окружения — провалы, ниши, окклюзионная тень.
+ # Шкала шире, чем у ON: провалы по дальности много глубже выступов.
+ off = _norm(lam.off, 0.0, 0.06)
+ off_img = cv2.applyColorMap((off * 255).astype(np.uint8), cv2.COLORMAP_BONE)
+ off_img[~tf.valid] = (34, 32, 38)
+ self._panel(off_img, "ламина L2 · OFF-контраст", "дальше фона", 2, out)
+
+ # 4. Кандидаты: лучи, попавшие в габарит, поверх приглушённой сцены.
+ # Цвет по новизне: знакомое тусклое, незнакомое горит.
+ base = np.where(tf.valid, 40 + depth * 95, 22).astype(np.uint8)
+ cand = cv2.cvtColor(base, cv2.COLOR_GRAY2BGR)
+ mark = np.zeros(cand.shape[:2], np.uint8)
+ hot_of = np.zeros(cand.shape[:2], np.float32)
+ for c in res.candidates:
+ rays = c.extra.get("rays") if isinstance(c.extra, dict) else None
+ if rays is None:
+ continue
+ ii, jj = rays
+ keep = (ii < cand.shape[0]) & (jj < cand.shape[1])
+ mark[ii[keep], jj[keep]] = 1
+ hot_of[ii[keep], jj[keep]] = float(np.clip(c.novelty, 0.0, 1.0))
+ if mark.any():
+ # утолщаем: одиночные лучи на уменьшенной панели не видно
+ k = np.ones((3, 5), np.uint8)
+ grown = cv2.dilate(mark, k)
+ hot_g = cv2.dilate(hot_of, k)
+ m = grown.astype(bool)
+ cand[m] = np.stack([np.full(m.sum(), 60),
+ (110 + 130 * hot_g[m]).astype(int),
+ (70 + 185 * hot_g[m]).astype(int)], axis=1)
+ self._panel(cand, "лобула LC11 · кандидаты",
+ f"{len(res.candidates)} шт.", 3, out)
+
+ s = self.s
+ cv2.line(out, (PAD * s, self.strip_h - 2 * s), (self.w - PAD * s, self.strip_h - 2 * s),
+ (52, 52, 62), s)
+ return out
+
+ # ------------------------------------------------------------------ кадр
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ cloud = self.cloud.render(res)
+ if cloud is None:
+ return None
+ out = np.empty((self.h, self.w, 3), np.uint8)
+ out[:self.strip_h] = self._strip(res)
+ out[self.strip_h:] = cloud
+ return out
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from .brain_view import BrainView
+ return BrainView.to_msg(img, stamp, frame_id)
diff --git a/flyguard/brain_view.py b/flyguard/brain_view.py
new file mode 100644
index 0000000..0f7ac1e
--- /dev/null
+++ b/flyguard/brain_view.py
@@ -0,0 +1,369 @@
+"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
+
+Каждая стадия FlyGuard соответствует конкретному нейропилю, поэтому состояние
+системы показывается не графиками, а анатомической схемой: где сейчас «горит»,
+там и идёт работа. На демонстрации это замыкает цепочку из ТЗ —
+тоннель → облако точек → мозг мухи → «препятствие, 87 м».
+
+Вид дорсальный, схематичный, но с правильным взаимным расположением: по краям
+оптические доли (ламина → медулла → лобула и лобулярная пластинка), в центре
+грибовидные тела (чашечка → ножка → доли) и центральный комплекс
+(протоцеребральный мост, веерное тело, эллипсоидное тело, нодули), снизу —
+нисходящие нейроны с гигантским волокном.
+
+Отрисовка стоит 2–4 мс и вынесена под флаг `brain_view`, чтобы не попадать
+в замеры задержки основного тракта.
+"""
+from __future__ import annotations
+
+import array
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError: # без OpenCV вид просто выключен
+ cv2 = None
+
+# Встроенные шрифты OpenCV (Hershey) знают только латиницу: любая кириллица
+# выходит рядом вопросительных знаков. Подписи здесь русские и составляют
+# половину смысла схемы, поэтому текст рисуется через PIL шрифтом DejaVu.
+try:
+ from PIL import Image, ImageDraw, ImageFont
+except ImportError:
+ Image = ImageDraw = ImageFont = None
+
+_FONT_PATHS = (
+ "/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
+ "/usr/share/fonts/truetype/dejavu/DejaVuSansCondensed.ttf",
+ "/usr/share/fonts/TTF/DejaVuSans.ttf",
+ "C:/Windows/Fonts/segoeui.ttf",
+ "C:/Windows/Fonts/arial.ttf",
+)
+_font_cache: dict[int, object] = {}
+
+
+def _font(px: int):
+ """Шрифт нужного кегля; None — если ни одного файла не нашлось."""
+ if ImageFont is None:
+ return None
+ if px not in _font_cache:
+ f = None
+ for path in _FONT_PATHS:
+ try:
+ f = ImageFont.truetype(path, px)
+ break
+ except OSError:
+ continue
+ _font_cache[px] = f or ImageFont.load_default()
+ return _font_cache[px]
+
+
+def _px(scale: float) -> int:
+ """Кегль PIL, дающий примерно ту же высоту, что и Hershey с этим масштабом."""
+ return max(9, int(round(scale * 30)))
+
+
+def text_size(txt: str, scale: float) -> tuple[int, int]:
+ f = _font(_px(scale))
+ if f is None:
+ (tw, th), _ = cv2.getTextSize(txt, cv2.FONT_HERSHEY_SIMPLEX, scale, 1)
+ return tw, th
+ box = f.getbbox(txt)
+ return box[2] - box[0], box[3] - box[1]
+
+
+def put_text(img, txt: str, org, scale: float, colour, weight: int = 1) -> None:
+ """Надпись с кириллицей. `org` — левый нижний угол, как у cv2.putText.
+
+ Рисуется по вырезанному прямоугольнику, а не по всему полотну: PIL копирует
+ изображение при каждом преобразовании, и на полном кадре полтора десятка
+ подписей стоили 90 мс — больше, чем весь конвейер.
+ """
+ f = _font(_px(scale))
+ if f is None or Image is None:
+ cv2.putText(img, txt, org, cv2.FONT_HERSHEY_SIMPLEX, scale, colour,
+ weight, cv2.LINE_AA)
+ return
+ x, y = int(org[0]), int(org[1])
+ box = f.getbbox(txt)
+ pad = 2
+ # Чернила лягут в x … x+(box[2]-box[0]) по горизонтали и
+ # y-(box[3]-box[1]) … y по вертикали — вырез берётся ровно по ним.
+ x0 = max(x - pad, 0)
+ y0 = max(y - (box[3] - box[1]) - pad, 0)
+ x1 = min(x + (box[2] - box[0]) + pad + 1, img.shape[1])
+ y1 = min(y + pad + 1, img.shape[0])
+ if x1 <= x0 or y1 <= y0:
+ return
+ crop = img[y0:y1, x0:x1]
+ pil = Image.fromarray(crop[:, :, ::-1])
+ ImageDraw.Draw(pil).text((x - box[0] - x0, y - box[3] - y0), txt, font=f,
+ fill=tuple(int(c) for c in colour[::-1]))
+ crop[:, :, :] = np.asarray(pil)[:, :, ::-1]
+
+W, H = 1180, 620
+BG = (22, 20, 26)
+INK = (150, 150, 165)
+
+# шкала «покой → работа → тревога» в BGR
+_STOPS = (
+ (0.00, (58, 48, 44)),
+ (0.30, (120, 96, 52)),
+ (0.55, (168, 178, 64)),
+ (0.78, (72, 196, 240)),
+ (1.00, (66, 66, 248)),
+)
+
+
+def _colour(a: float) -> tuple[int, int, int]:
+ a = float(np.clip(a, 0.0, 1.0))
+ for (x0, c0), (x1, c1) in zip(_STOPS, _STOPS[1:]):
+ if a <= x1:
+ t = (a - x0) / max(x1 - x0, 1e-6)
+ return tuple(int(c0[i] + (c1[i] - c0[i]) * t) for i in range(3))
+ return _STOPS[-1][1]
+
+
+def _annulus(cx, cy, r_out, r_in, a0, a1, squash=1.0):
+ """Кольцевой сектор — форма слоя оптической доли."""
+ # ellipse2Poly требует положительный шаг, поэтому внутренняя дуга строится
+ # в прямом направлении и разворачивается уже массивом
+ outer = cv2.ellipse2Poly((cx, cy), (r_out, int(r_out * squash)), 0, a0, a1, 4)
+ inner = cv2.ellipse2Poly((cx, cy), (r_in, int(r_in * squash)), 0, a0, a1, 4)
+ return np.vstack([outer, inner[::-1]])
+
+
+class Region:
+ __slots__ = ("key", "ru", "pts", "anchor", "label_at")
+
+ def __init__(self, key, ru, pts, label_at=None):
+ self.key = key
+ self.ru = ru
+ self.pts = np.asarray(pts, np.int32)
+ self.anchor = tuple(self.pts.mean(axis=0).astype(int))
+ self.label_at = label_at
+
+
+def _build_regions() -> list[Region]:
+ r: list[Region] = []
+ for side, (cx, a0, a1, lx) in enumerate(((250, 118, 242, 18), (W - 250, -62, 62, W - 250))):
+ cy = 300
+ mirror = side == 1
+ spec = [
+ ("lamina", "ламина L1 / L2", 218, 188),
+ ("medulla", "медулла T4 / T5", 182, 132),
+ ("lobula", "лобула LC11", 126, 88),
+ ("lobula_plate", "лоб. пластинка LPTC / LPLC2", 82, 52),
+ ]
+ for i, (key, ru, ro, ri) in enumerate(spec):
+ pts = _annulus(cx, cy, ro, ri, a0 + i * 3, a1 - i * 3, squash=1.06)
+ ly = 96 + i * 26
+ r.append(Region(key, ru, pts, (30 if not mirror else W - 330, ly)))
+
+ cx = W // 2
+
+ # грибовидные тела: чашечка сзади → ножка вперёд → вертикальная и медиальная
+ # доли. Они лежат ЛАТЕРАЛЬНЕЕ центрального комплекса, не поверх него.
+ for mirror in (False, True):
+ s = -1 if mirror else 1
+ bx = cx + s * 150
+ calyx = cv2.ellipse2Poly((bx, 132), (46, 33), 0, 0, 360, 6)
+ ped = np.array([[bx - s * 16, 160], [bx + s * 18, 158],
+ [bx + s * 46, 318], [bx + s * 18, 322]], np.int32)
+ lobes = np.array([[bx + s * 16, 316], [bx + s * 50, 314], [bx + s * 56, 404],
+ [bx + s * 26, 408], [bx + s * 26, 372], [bx + s * 16, 372]],
+ np.int32)
+ r.append(Region("mb_calyx", "чашечка PN → KC", calyx,
+ (cx - 330, 86) if not mirror else (cx + 210, 86)))
+ r.append(Region("mb_ped", "ножка · APL", ped, None))
+ r.append(Region("mb_lobes", "доли KC → MBON (новизна)", lobes,
+ (cx - 400, 432) if not mirror else (cx + 230, 432)))
+
+ pb = _annulus(cx, 232, 104, 86, 200, 340, squash=0.7)
+ fb = np.array([[cx - 74, 262], [cx + 74, 262], [cx + 58, 314], [cx - 58, 314]], np.int32)
+ eb = _annulus(cx, 372, 46, 26, 0, 360)
+ no_l = cv2.ellipse2Poly((cx - 52, 420), (18, 13), 0, 0, 360, 8)
+ no_r = cv2.ellipse2Poly((cx + 52, 420), (18, 13), 0, 0, 360, 8)
+ dn = np.array([[cx - 22, 444], [cx + 22, 444], [cx + 40, 566], [cx - 40, 566]], np.int32)
+
+ r += [
+ Region("pb", "протоцеребральный мост", pb, (cx - 96, 176)),
+ Region("fb", "веерное тело", fb, (cx - 40, 342)),
+ Region("eb", "эллипсоидное тело — кольцевой аттрактор", eb, (cx - 300, 372)),
+ Region("no", "нодули", no_l, None),
+ Region("no", "нодули", no_r, (cx + 96, 428)),
+ Region("dn", "нисходящие нейроны · гигантское волокно", dn, (cx - 300, 524)),
+ ]
+ return r
+
+
+class BrainView:
+ """Отрисовка схемы с активностью."""
+
+ def __init__(self, labels: bool = True):
+ self.enabled = cv2 is not None
+ self.labels = labels
+ self.regions = _build_regions() if self.enabled else []
+ self._base = self._draw_base() if self.enabled else None
+ self._smooth: dict[str, float] = {}
+
+ # ------------------------------------------------------------------ статика
+
+ def _draw_base(self) -> np.ndarray:
+ img = np.full((H, W, 3), BG, np.uint8)
+ put_text(img, "FlyGuard — схемы мозга Drosophila melanogaster в реальном времени",
+ (26, 36), 0.66, (205, 205, 215))
+ cv2.line(img, (26, 48), (W - 26, 48), (52, 52, 62), 1)
+ return img
+
+ # ------------------------------------------------------------------ активность
+
+ @staticmethod
+ def activity(res) -> dict[str, float]:
+ """Свести состояние кадра к активности по нейропилям, 0…1."""
+ lam = res.lam
+ d = res.decision
+ on = float(np.mean(lam.on)) * 130.0 if lam is not None else 0.0
+ off = float(np.mean(lam.off)) * 130.0 if lam is not None else 0.0
+ hole = float(np.mean(lam.hole)) if lam is not None else 0.0
+ speed = abs(res.ego.speed) / 20.0 if res.ego else 0.0
+ loom = (float(np.percentile(res.loom, 99.9)) * 25.0
+ if res.loom is not None and res.loom.size else 0.0)
+ nov = max((c.novelty for c in res.candidates), default=0.0)
+ # Дофамин: у мухи нейроны PPL1/PAM управляют пластичностью синапсов
+ # KC→MBON — ровно тем, чем учится наша память тоннеля. Осмысленная
+ # величина здесь — сила сигнала обучения: насколько сцена в целом
+ # незнакома памяти. В отличие от `nov` (самый незнакомый предмет)
+ # это средняя незнакомость всего, что видно, и она заранее говорит,
+ # что участок новый и ложных тревог будет больше.
+ dopa = (float(np.mean([c.novelty for c in res.candidates]))
+ if res.candidates else 0.0)
+ n_cand = min(len(res.candidates) / 7.0, 1.0)
+ evid = max((o.confidence for o in d.objects), default=0.0)
+
+ valid = float(lam.disp.astype(bool).mean()) if lam is not None else 0.0
+
+ return {
+ # Сетчатка «горит» тем сильнее, чем больше лучей вернулось: это и
+ # есть освещённость омматидиальной решётки.
+ "retina": np.clip(0.15 + 0.85 * valid, 0, 1),
+ "looming": np.clip(loom, 0, 1),
+ "dopamine": np.clip(dopa, 0, 1),
+ # Привыкание: доля синапсов KC→MBON, уже подавленных повторами
+ # внутри этого проезда. Ноль в начале участка, растёт по мере
+ # того, как однообразная обстановка перестаёт быть новостью.
+ "habituation": np.clip(getattr(res, "hab", 0.0), 0, 1),
+ "lamina": np.clip(0.55 * on + 0.45 * off, 0, 1),
+ "medulla": np.clip(0.25 + 0.75 * speed, 0, 1),
+ "lobula": np.clip(n_cand, 0, 1),
+ "lobula_plate": np.clip(max(speed, loom), 0, 1),
+ "mb_calyx": np.clip(n_cand, 0, 1),
+ "mb_ped": np.clip(0.4 * n_cand, 0, 1),
+ "mb_lobes": np.clip(nov, 0, 1),
+ "pb": np.clip(speed, 0, 1),
+ "fb": np.clip(hole, 0, 1),
+ "eb": np.clip(evid, 0, 1),
+ "no": np.clip(abs(res.ego.yaw_deg) / 1.5 if res.ego else 0.0, 0, 1),
+ "dn": 1.0 if d.emergency else (0.7 if d.detected else 0.06),
+ }
+
+ # ------------------------------------------------------------------ кадр
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ for k, v in self.activity(res).items(): # сглаживание: мозг не мигает
+ self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
+
+ img = self._base.copy()
+ for reg in self.regions:
+ a = self._smooth.get(reg.key, 0.0)
+ cv2.fillPoly(img, [reg.pts], _colour(a))
+ cv2.polylines(img, [reg.pts], True, (46, 44, 54), 1, cv2.LINE_AA)
+
+ self._draw_bump(img, res)
+ self._draw_flow(img)
+ if self.labels:
+ self._draw_labels(img)
+ self._draw_readout(img, res)
+ return img
+
+ def _draw_labels(self, img) -> None:
+ seen = set()
+ for reg in self.regions:
+ if reg.label_at is None or reg.ru in seen:
+ continue
+ seen.add(reg.ru)
+ x, y = reg.label_at
+ tw, th = text_size(reg.ru, 0.44)
+ # подложка под текстом: подписи неизбежно ложатся поверх нейропилей,
+ # и без неё их не прочитать
+ box = img[max(y - th - 6, 0):y + 6, max(x - 6, 0):x + tw + 6]
+ if box.size:
+ box[:] = (box * 0.25).astype(np.uint8)
+ cv2.line(img, (x + tw // 2, y - th - 6 if y > reg.anchor[1] else y + 6),
+ reg.anchor, (74, 72, 86), 1, cv2.LINE_AA)
+ put_text(img, reg.ru, (x, y), 0.44, INK)
+
+ def _draw_bump(self, img, res) -> None:
+ """Бугор активности в эллипсоидном теле — направление на ближайший объект."""
+ cx, cy, r = W // 2, 380, 36
+ objs = res.decision.objects
+ if not objs:
+ return
+ o = objs[0]
+ ang = float(np.clip(np.arctan2(o.lateral, max(o.distance, 1.0)) * 7.0, -3.0, 3.0))
+ px = int(cx + r * np.sin(ang))
+ py = int(cy - r * np.cos(ang))
+ cv2.circle(img, (px, py), 11, _colour(min(o.confidence + 0.35, 1.0)), -1, cv2.LINE_AA)
+ cv2.circle(img, (px, py), 11, (230, 230, 240), 1, cv2.LINE_AA)
+
+ def _draw_flow(self, img) -> None:
+ cx = W // 2
+ for a, b in (((330, 300), (410, 260)), ((W - 330, 300), (W - 410, 260)),
+ ((cx - 60, 336), (cx - 30, 356)), ((cx + 60, 336), (cx + 30, 356)),
+ ((cx, 420), (cx, 428))):
+ cv2.arrowedLine(img, a, b, (86, 84, 98), 1, cv2.LINE_AA, tipLength=0.3)
+
+ def _draw_readout(self, img, res) -> None:
+ d = res.decision
+ rows = [
+ ("скорость", f"{res.ego.kmh:.1f} км/ч" if res.ego else "—"),
+ ("кандидатов", str(len(res.candidates))),
+ ("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
+ ("треков", str(len(d.objects))),
+ ("обработка", f"{res.total_ms:.1f} мс"),
+ ]
+ y0 = H - 150
+ for i, (k, v) in enumerate(rows):
+ put_text(img, k, (28, y0 + i * 24), 0.46, (118, 116, 128))
+ put_text(img, v, (170, y0 + i * 24), 0.5, (206, 206, 216))
+
+ if d.detected:
+ txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
+ sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
+ else f"уверенность {d.confidence:.2f}")
+ colour = (66, 66, 248) if d.emergency else (72, 196, 240)
+ else:
+ txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
+ put_text(img, txt, (W - 470, H - 90), 0.95, colour, 2)
+ if sub:
+ put_text(img, sub, (W - 470, H - 58), 0.56, colour)
+
+ # ------------------------------------------------------------------ ROS
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from sensor_msgs.msg import Image
+ msg = Image()
+ msg.header.stamp = stamp
+ msg.header.frame_id = frame_id
+ msg.height, msg.width = img.shape[:2]
+ msg.encoding = "bgr8"
+ msg.is_bigendian = 0
+ msg.step = img.shape[1] * 3
+ # array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python,
+ # и картинка в 2.8 МБ стоила 142 мс на кадр против 2.4 мс (замерено).
+ msg.data = array.array("B", img.tobytes())
+ return msg
diff --git a/flyguard/data/brain_atlas.npz b/flyguard/data/brain_atlas.npz
new file mode 100644
index 0000000..03010ba
Binary files /dev/null and b/flyguard/data/brain_atlas.npz differ
diff --git a/flyguard/data/brain_atlas_x2.npz b/flyguard/data/brain_atlas_x2.npz
new file mode 100644
index 0000000..deae590
Binary files /dev/null and b/flyguard/data/brain_atlas_x2.npz differ
diff --git a/flyguard/data/brain_atlas_x3.npz b/flyguard/data/brain_atlas_x3.npz
new file mode 100644
index 0000000..7c62e17
Binary files /dev/null and b/flyguard/data/brain_atlas_x3.npz differ
diff --git a/flyguard/data/pandar128_channels.csv b/flyguard/data/pandar128_channels.csv
new file mode 100644
index 0000000..85be719
--- /dev/null
+++ b/flyguard/data/pandar128_channels.csv
@@ -0,0 +1,129 @@
+channel,az_offset_deg,elevation_deg,range_min_m,range_max_m,near_field,max_range_10pct_m,far_field,min_reflectivity,high_res
+1,3.257,14.436,0.3,100.0,1,100.0,0,100 m @ 10%,0
+2,3.263,13.535,2.7,100.0,0,100.0,0,100 m @ 10%,0
+3,1.091,13.082,0.3,100.0,1,100.0,0,100 m @ 10%,0
+4,3.268,12.624,2.7,100.0,0,100.0,0,100 m @ 10%,0
+5,1.093,12.165,2.7,100.0,0,100.0,0,100 m @ 10%,0
+6,3.273,11.702,0.3,100.0,1,120.0,0,100 m @ 6%,0
+7,1.094,11.239,2.7,100.0,0,120.0,0,100 m @ 6%,0
+8,3.278,10.771,2.7,100.0,0,140.0,0,100 m @ 3%,0
+9,1.095,10.305,0.3,100.0,1,140.0,0,100 m @ 3%,0
+10,3.283,9.83,2.7,100.0,0,140.0,0,100 m @ 3%,0
+11,1.096,9.356,2.7,100.0,0,140.0,0,100 m @ 3%,0
+12,3.288,8.88,0.3,100.0,1,140.0,0,100 m @ 3%,0
+13,1.097,8.401,2.7,100.0,0,140.0,0,100 m @ 3%,0
+14,3.291,7.921,2.7,100.0,0,140.0,0,100 m @ 3%,0
+15,1.098,7.438,0.3,100.0,1,140.0,0,100 m @ 3%,0
+16,-1.101,6.953,2.7,100.0,0,140.0,0,100 m @ 3%,0
+17,1.1,6.467,2.7,100.0,0,140.0,0,100 m @ 3%,0
+18,-1.104,5.978,0.3,100.0,1,140.0,0,100 m @ 3%,0
+19,-3.306,5.487,2.7,100.0,0,140.0,0,100 m @ 3%,0
+20,-1.106,4.996,2.7,100.0,0,140.0,0,100 m @ 3%,0
+21,-3.311,4.501,0.3,100.0,1,140.0,0,100 m @ 3%,0
+22,-1.109,4.007,2.7,100.0,0,140.0,0,100 m @ 3%,0
+23,-3.318,3.509,2.7,100.0,0,140.0,0,100 m @ 3%,0
+24,-1.111,3.013,0.3,100.0,1,140.0,0,100 m @ 3%,0
+25,-3.324,2.512,2.7,100.0,0,140.0,0,100 m @ 3%,0
+26,-1.113,2.013,0.3,200.0,1,140.0,0,200 m @ 37%,1
+27,7.72,1.885,2.7,200.0,0,140.0,0,200 m @ 37%,1
+28,5.535,1.761,2.7,200.0,0,140.0,0,200 m @ 37%,1
+29,3.325,1.637,2.7,200.0,0,140.0,0,200 m @ 37%,1
+30,-3.33,1.511,2.7,200.0,0,140.0,0,200 m @ 37%,1
+31,1.107,1.386,2.7,200.0,0,140.0,0,200 m @ 37%,1
+32,-5.538,1.258,2.7,200.0,0,140.0,0,200 m @ 37%,1
+33,-7.726,1.13,0.3,200.0,1,140.0,0,200 m @ 37%,1
+34,-1.115,1.008,2.7,200.0,0,200.0,1,200 m @ 10%,1
+35,7.731,0.88,2.7,200.0,0,200.0,1,200 m @ 10%,1
+36,5.543,0.756,2.7,200.0,0,200.0,1,200 m @ 10%,1
+37,3.329,0.63,2.7,200.0,0,200.0,1,200 m @ 10%,1
+38,-3.336,0.505,2.7,200.0,0,200.0,1,200 m @ 10%,1
+39,1.108,0.379,2.7,200.0,0,200.0,1,200 m @ 10%,1
+40,-5.547,0.251,0.3,200.0,1,200.0,1,200 m @ 10%,1
+41,-7.738,0.124,2.7,200.0,0,200.0,1,200 m @ 10%,1
+42,-1.117,0.0,2.7,200.0,0,200.0,1,200 m @ 10%,1
+43,7.743,-0.129,2.7,200.0,0,200.0,1,200 m @ 10%,1
+44,5.551,-0.254,2.7,200.0,0,200.0,1,200 m @ 10%,1
+45,3.335,-0.38,2.7,200.0,0,200.0,1,200 m @ 10%,1
+46,-3.342,-0.506,2.7,200.0,0,200.0,1,200 m @ 10%,1
+47,1.11,-0.632,0.3,200.0,1,200.0,1,200 m @ 10%,1
+48,-5.555,-0.76,2.7,200.0,0,200.0,1,200 m @ 10%,1
+49,-7.75,-0.887,2.7,200.0,0,200.0,1,200 m @ 10%,1
+50,-1.119,-1.012,2.7,200.0,0,200.0,1,200 m @ 10%,1
+51,7.757,-1.141,2.7,200.0,0,200.0,1,200 m @ 10%,1
+52,5.56,-1.266,2.7,200.0,0,200.0,1,200 m @ 10%,1
+53,3.34,-1.393,2.7,200.0,0,200.0,1,200 m @ 10%,1
+54,-3.347,-1.519,0.3,200.0,1,200.0,1,200 m @ 10%,1
+55,1.111,-1.646,2.7,200.0,0,200.0,1,200 m @ 10%,1
+56,-5.564,-1.773,2.7,200.0,0,200.0,1,200 m @ 10%,1
+57,-7.762,-1.901,2.7,200.0,0,200.0,1,200 m @ 10%,1
+58,-1.121,-2.027,2.7,200.0,0,200.0,1,200 m @ 10%,1
+59,7.768,-2.155,2.7,200.0,0,200.0,1,200 m @ 10%,1
+60,5.569,-2.282,2.7,200.0,0,200.0,1,200 m @ 10%,1
+61,3.345,-2.409,0.3,200.0,1,200.0,1,200 m @ 10%,1
+62,-3.353,-2.535,2.7,200.0,0,200.0,1,200 m @ 10%,1
+63,1.113,-2.663,2.7,200.0,0,200.0,1,200 m @ 10%,1
+64,-5.573,-2.789,2.7,200.0,0,200.0,1,200 m @ 10%,1
+65,-7.775,-2.916,2.7,200.0,0,200.0,1,200 m @ 10%,1
+66,-1.123,-3.044,2.7,200.0,0,140.0,0,200 m @ 37%,1
+67,7.78,-3.172,2.7,200.0,0,140.0,0,200 m @ 37%,1
+68,5.578,-3.299,0.3,200.0,1,140.0,0,200 m @ 37%,1
+69,3.351,-3.425,2.7,200.0,0,140.0,0,200 m @ 37%,1
+70,-3.358,-3.552,2.7,200.0,0,140.0,0,200 m @ 37%,1
+71,1.115,-3.68,2.7,200.0,0,140.0,0,200 m @ 37%,1
+72,-5.582,-3.806,2.7,200.0,0,140.0,0,200 m @ 37%,1
+73,-7.787,-3.933,2.7,200.0,0,140.0,0,200 m @ 37%,1
+74,-1.125,-4.062,2.7,200.0,0,140.0,0,200 m @ 37%,1
+75,7.792,-4.19,0.3,200.0,1,140.0,0,200 m @ 37%,1
+76,5.586,-4.318,2.7,200.0,0,140.0,0,200 m @ 37%,1
+77,3.356,-4.444,2.7,200.0,0,140.0,0,200 m @ 37%,1
+78,-3.363,-4.571,2.7,200.0,0,140.0,0,200 m @ 37%,1
+79,1.116,-4.699,2.7,200.0,0,140.0,0,200 m @ 37%,1
+80,-5.591,-4.824,2.7,200.0,0,140.0,0,200 m @ 37%,1
+81,-7.799,-4.951,2.7,200.0,0,140.0,0,200 m @ 37%,1
+82,-1.127,-5.081,0.3,200.0,1,140.0,0,200 m @ 37%,1
+83,7.804,-5.209,2.7,200.0,0,140.0,0,200 m @ 37%,1
+84,5.595,-5.336,2.7,200.0,0,140.0,0,200 m @ 37%,1
+85,3.36,-5.463,2.7,200.0,0,140.0,0,200 m @ 37%,1
+86,-3.369,-5.589,2.7,200.0,0,140.0,0,200 m @ 37%,1
+87,1.118,-5.718,2.7,200.0,0,140.0,0,200 m @ 37%,1
+88,-5.599,-5.843,2.7,200.0,0,140.0,0,200 m @ 37%,1
+89,-7.811,-5.968,2.7,200.0,0,140.0,0,200 m @ 37%,1
+90,-1.129,-6.1,0.3,100.0,1,140.0,0,100 m @ 3%,0
+91,-3.374,-6.607,2.7,100.0,0,140.0,0,100 m @ 3%,0
+92,-1.13,-7.117,2.7,100.0,0,140.0,0,100 m @ 3%,0
+93,-3.379,-7.624,0.3,100.0,1,140.0,0,100 m @ 3%,0
+94,-1.132,-8.134,2.7,100.0,0,140.0,0,100 m @ 3%,0
+95,-3.383,-8.64,2.7,100.0,0,140.0,0,100 m @ 3%,0
+96,3.381,-9.149,0.3,100.0,1,140.0,0,100 m @ 3%,0
+97,-3.388,-9.652,2.7,100.0,0,140.0,0,100 m @ 3%,0
+98,3.386,-10.16,2.7,100.0,0,100.0,0,100 m @ 10%,0
+99,1.129,-10.665,0.3,100.0,1,100.0,0,100 m @ 10%,0
+100,3.39,-11.17,2.7,100.0,0,100.0,0,100 m @ 10%,0
+101,1.129,-11.672,2.7,100.0,0,100.0,0,100 m @ 10%,0
+102,3.395,-12.174,0.3,100.0,1,100.0,0,100 m @ 10%,0
+103,1.131,-12.673,2.7,100.0,0,100.0,0,100 m @ 10%,0
+104,3.401,-13.173,2.7,100.0,0,100.0,0,100 m @ 10%,0
+105,1.133,-13.67,0.3,100.0,1,100.0,0,100 m @ 10%,0
+106,3.406,-14.166,2.7,100.0,0,50.0,0,100 m @ 120%,0
+107,1.135,-14.66,2.7,100.0,0,50.0,0,100 m @ 120%,0
+108,3.41,-15.154,0.3,100.0,1,50.0,0,100 m @ 120%,0
+109,1.137,-15.645,2.7,100.0,0,50.0,0,100 m @ 120%,0
+110,3.416,-16.135,2.7,100.0,0,50.0,0,100 m @ 120%,0
+111,1.139,-16.622,0.3,100.0,1,50.0,0,100 m @ 120%,0
+112,-1.142,-17.106,2.7,100.0,0,50.0,0,100 m @ 120%,0
+113,1.142,-17.592,2.7,100.0,0,50.0,0,100 m @ 120%,0
+114,-1.143,-18.072,0.3,100.0,1,50.0,0,100 m @ 120%,0
+115,-3.426,-18.548,2.7,100.0,0,50.0,0,100 m @ 120%,0
+116,-1.143,-19.03,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+117,-3.429,-19.501,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+118,-1.145,-19.978,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+119,-3.433,-20.445,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+120,-1.145,-20.918,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+121,-3.436,-21.379,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+122,-1.146,-21.848,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+123,-3.44,-22.304,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+124,-1.146,-22.768,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+125,-3.443,-23.219,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+126,-1.146,-23.678,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+127,-3.446,-24.123,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+128,-3.449,-25.016,0.3,100.0,1,25.0,0,100 m @ 1600%,0
diff --git a/flyguard/node.py b/flyguard/node.py
new file mode 100644
index 0000000..de0c797
--- /dev/null
+++ b/flyguard/node.py
@@ -0,0 +1,708 @@
+"""ROS 2-нода FlyGuard.
+
+Подписывается на облако точек лидара, прогоняет конвейер и публикует:
+
+ /flyguard/obstacle flyguard_msgs/ObstacleStatus — главный программный выход
+ /flyguard/markers visualization_msgs/MarkerArray — рамки объектов и габарит
+ /flyguard/view_cloud sensor_msgs/PointCloud2 — облако обзора для RViz
+ /flyguard/debug_cloud sensor_msgs/PointCloud2 — раскраска по новизне
+ /flyguard/brain sensor_msgs/Image — схема мозга мухи с активностью
+ /flyguard/diagnostics diagnostic_msgs/DiagnosticArray — задержки по стадиям
+
+Обработка идёт в отдельном потоке, и из очереди всегда берётся **последний**
+пришедший кадр: система реального времени обязана отвечать на текущую обстановку,
+а не доедать накопившееся прошлое.
+"""
+from __future__ import annotations
+
+import array
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+from geometry_msgs.msg import Point, TransformStamped
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Bool, Float32, Header
+from tf2_ros import StaticTransformBroadcaster
+from visualization_msgs.msg import Marker, MarkerArray
+
+from flyguard_msgs.msg import DetectedObject, ObstacleStatus
+
+from . import ros_conv
+from .cdr import parse_pointcloud2
+from .export import gauge_outline
+from .mushroom_body import MushroomBody
+from .pipeline import FlyGuard, Params
+
+
+class FlyGuardNode(Node):
+ def __init__(self):
+ super().__init__("flyguard")
+ p = self.declare_parameters("", [
+ ("input_topic", "/lidar_points"),
+ ("fallback_topics", ["/sensing/lidar/hesai128/pointcloud", "/points_raw"]),
+ ("frame_id", ""),
+ ("best_effort", True),
+ ("raw_subscription", True),
+ ("queue_depth", 20),
+ ("async_worker", False),
+ ("memory_path", ""),
+ ("mbon_path", ""),
+ ("enable_mbon", True),
+ # плотные стадии (сетчатка, ламина, кластеризация): auto — видеокарта,
+ # если PyTorch её видит, иначе процессор; cuda; cpu
+ ("device", "auto"),
+ ("mbon_power", 1.5),
+ ("mbon_blend", 1.0),
+ ("fov_deg", 30.0),
+ ("half_width", 1.2),
+ ("h_lo", 0.28),
+ ("h_hi", 2.3),
+ ("h_top", 3.3),
+ ("half_width_top", 1.0),
+ ("top_d_max", 90.0),
+ ("h_lo_core", 0.16),
+ ("core_from", 30.0),
+ ("ctx_up", 4.0),
+ ("split_adv", 0.0),
+ ("split_gap", 6.0),
+ ("split_near", 55.0),
+ ("split_top", 1),
+ ("enable_accumulator", True),
+ ("acc_near", 55.0),
+ ("acc_gain", 1.5),
+ ("enable_habituation", False),
+ ("hab_rate", 0.25),
+ ("hab_place_m", 5.0),
+ ("hab_recover_m", 800.0),
+ ("d_min", 4.0),
+ ("d_max", 220.0),
+ ("min_rays", 4),
+ ("publish_debug_cloud", False),
+ ("publish_markers", True),
+ ("brain_view", False),
+ # схема | облако нейронов коннектома | гибрид (панели + облако)
+ ("brain_style", "hybrid"),
+ ("brain_period", 0.2),
+ # 1 — 1180 пикселей по ширине; 2–3 — для экрана и видео в 2K/4K
+ ("brain_scale", 1),
+ ])
+ self.par = {q.name: q.value for q in p}
+
+ brain_on = bool(self.par["brain_view"])
+ params = Params(device=str(self.par["device"]),
+ fov_deg=float(self.par["fov_deg"]),
+ half_width=float(self.par["half_width"]),
+ h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
+ h_top=float(self.par["h_top"]),
+ half_width_top=float(self.par["half_width_top"]),
+ top_d_max=float(self.par["top_d_max"]),
+ h_lo_core=float(self.par["h_lo_core"]),
+ core_from=float(self.par["core_from"]),
+ ctx_up=float(self.par["ctx_up"]),
+ split_adv=float(self.par["split_adv"]),
+ split_gap=float(self.par["split_gap"]),
+ split_near=float(self.par["split_near"]),
+ split_top=int(self.par["split_top"]),
+ enable_accumulator=bool(self.par["enable_accumulator"]),
+ acc_near=float(self.par["acc_near"]),
+ acc_gain=float(self.par["acc_gain"]),
+ enable_habituation=bool(self.par["enable_habituation"]),
+ hab_rate=float(self.par["hab_rate"]),
+ hab_place_m=float(self.par["hab_place_m"]),
+ hab_recover_m=float(self.par["hab_recover_m"]),
+ d_min=float(self.par["d_min"]), d_max=float(self.par["d_max"]),
+ min_rays=int(self.par["min_rays"]),
+ # каналы T4/T5 и LPLC2 считаются только когда есть кому
+ # их показать: на решение они пока не влияют
+ enable_mbon=bool(self.par["enable_mbon"]),
+ mbon_power=float(self.par["mbon_power"]),
+ mbon_blend=float(self.par["mbon_blend"]),
+ enable_looming=brain_on)
+
+ memory = None
+ mem_path = str(self.par["memory_path"])
+ if mem_path and Path(mem_path).exists():
+ memory = MushroomBody.load(mem_path)
+ self.get_logger().info(
+ f"память тоннеля загружена: {mem_path} "
+ f"({memory.cfg.n_kc} клеток Кеньона, обучена на {memory.n_seen} примерах)")
+ else:
+ self.get_logger().warn(
+ "память тоннеля не задана — штатные конструкции тоннеля не подавляются, "
+ "ложных тревог будет заметно больше")
+
+ readout = None
+ mb_path = str(self.par["mbon_path"])
+ if mb_path and Path(mb_path).exists():
+ from .mbon_readout import MbonReadout
+ readout = MbonReadout.load(mb_path)
+ self.get_logger().info(
+ f"считывание MBON загружено: {mb_path} "
+ f"({readout.cfg.n_kc} клеток Кеньона, {readout.n_pn} признаков)")
+ elif bool(self.par["enable_mbon"]):
+ self.get_logger().warn(
+ "считывание MBON не задано — вес улики считается ручной формулой, "
+ "ложных тревог будет больше")
+
+ # Видеокарта поднимается в фоне: подписка не ждёт прогрева ядер CUDA
+ self._t_start = time.monotonic()
+ self.fg = FlyGuard(params, memory=memory, readout=readout, gpu_background=True)
+ self._report_device()
+ self.brain = None
+ self.brain_period = float(self.par["brain_period"])
+ self._brain_last = 0.0
+ if brain_on:
+ style = str(self.par["brain_style"]).lower()
+ if style == "scheme":
+ from .brain_view import BrainView
+ self.brain = BrainView()
+ elif style == "cloud":
+ from .brain_atlas import NeuronCloud
+ self.brain = NeuronCloud(scale=int(self.par["brain_scale"]))
+ else:
+ from .brain_hybrid import BrainHybrid
+ self.brain = BrainHybrid(scale=int(self.par["brain_scale"]))
+ if not getattr(self.brain, "enabled", False):
+ # атласа нет — падать незачем, показываем схему
+ from .brain_view import BrainView
+ self.get_logger().warn(
+ f"вид «{style}» недоступен (нет атласа нейронов), беру схему")
+ self.brain = BrainView()
+
+ qos = QoSProfile(
+ history=QoSHistoryPolicy.KEEP_LAST, depth=int(self.par["queue_depth"]),
+ reliability=(QoSReliabilityPolicy.BEST_EFFORT if self.par["best_effort"]
+ else QoSReliabilityPolicy.RELIABLE),
+ durability=QoSDurabilityPolicy.VOLATILE)
+
+ # Кадр лидара — это 24 МБ, и сборка из них Python-объекта sensor_msgs
+ # стоит дороже всей нашей обработки: на записи с полным круговым сканом
+ # так терялась половина кадров. Поэтому по умолчанию берём сырые байты
+ # CDR и разбираем своим парсером — он строит numpy-вид поверх буфера
+ # без копирования. Обычный путь остаётся под флагом, на случай
+ # нестандартной раскладки полей.
+ self.raw = bool(self.par["raw_subscription"])
+ topics = list(dict.fromkeys([str(self.par["input_topic"])]
+ + list(self.par["fallback_topics"])))
+ self._qos = qos
+ self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, qos,
+ raw=self.raw)
+ for t in topics]
+ self.get_logger().info(
+ f"подписка на: {', '.join(topics)}"
+ f" ({'сырые байты CDR' if self.raw else 'разбор через rclpy'})")
+ # Сторож входа. Издатель на топике есть, а кадров нет — почти всегда это
+ # контейнер без --ipc host и `ros2 bag play` на хосте: Fast DDS шлёт
+ # кадры через /dev/shm, которой у них общей нет, и теряет их молча.
+ # Без подсказки такой запуск выглядит как пустой тоннель.
+ self._topics = topics
+ self._pub_seen_at: float | None = None
+ self._watch = self.create_timer(1.0, self._check_input)
+
+ self.pub_status = self.create_publisher(ObstacleStatus, "/flyguard/obstacle", 10)
+ self.pub_flag = self.create_publisher(Bool, "/flyguard/detected", 10)
+ self.pub_dist = self.create_publisher(Float32, "/flyguard/distance", 10)
+ self.pub_markers = self.create_publisher(MarkerArray, "/flyguard/markers", 5)
+ self.pub_diag = self.create_publisher(DiagnosticArray, "/flyguard/diagnostics", 5)
+ self.pub_cloud = self.create_publisher(PointCloud2, "/flyguard/debug_cloud", 2)
+ self.pub_view = self.create_publisher(PointCloud2, "/flyguard/view_cloud", 2)
+ self.pub_brain = None
+ if self.brain is not None:
+ from sensor_msgs.msg import Image
+ self.pub_brain = self.create_publisher(Image, "/flyguard/brain", 2)
+
+ # Имя кадра лидара в записях различается («lidar_livox», «hesai_lidar»),
+ # поэтому нода публикует свой вывод во всегда одинаковом кадре `lidar`
+ # и отдаёт статическое тождественное преобразование к пришедшему. Тогда
+ # один и тот же конфиг RViz работает с любым бэгом.
+ self.fixed_frame = "lidar"
+ self._tf = StaticTransformBroadcaster(self)
+ self._tf_sent: set[str] = set()
+ self._alarm = False # была ли тревога на прошлом кадре
+ self._alarm_frames = 0
+ self._jumps = 0 # скачки времени записи, о которых уже сказано
+
+ self._latest = None
+ self._lock = threading.Lock()
+ self._wake = threading.Event()
+ self._stop = False
+ self._dropped = 0
+ self._received = 0
+ self._cycle_ms = 0.0
+ self.async_worker = bool(self.par["async_worker"])
+ self._worker = None
+ if self.async_worker:
+ self._worker = threading.Thread(target=self._loop, daemon=True)
+ self._worker.start()
+
+ # ------------------------------------------------------------------ приём
+
+ def _check_input(self) -> None:
+ if self._received:
+ self._watch.cancel()
+ return
+ pubs = sum(self.count_publishers(t) for t in self._topics)
+ if not pubs:
+ self._adopt_foreign_cloud()
+ return
+ now = time.monotonic()
+ if self._pub_seen_at is None:
+ self._pub_seen_at = now
+ elif now - self._pub_seen_at >= 5.0:
+ self.get_logger().error(
+ f"на топике лидара есть издатель, а кадров нет уже "
+ f"{now - self._pub_seen_at:.0f} с. Если bag проигрывается на хосте, "
+ f"запустите контейнер с --ipc host: без него кадры идут через "
+ f"/dev/shm, общей у контейнера с хостом нет", throttle_duration_sec=10.0)
+
+ def _adopt_foreign_cloud(self) -> None:
+ """Облако идёт в топик, которого нет в списке, — подписаться и на него.
+
+ Имя топика у записей разное (в наших двух разное уже), и у контрольной
+ записи может оказаться третье. Узел, молча ждущий не тот топик, выглядит
+ как пустой тоннель. Свои топики `/flyguard/...` не в счёт.
+ """
+ for name, types in self.get_topic_names_and_types():
+ if (name in self._topics or name.startswith("/flyguard/")
+ or "sensor_msgs/msg/PointCloud2" not in types):
+ continue
+ self.get_logger().warn(
+ f"облако точек идёт в {name}, а узел слушал "
+ f"{', '.join(self._topics)} — подписываюсь и на него "
+ f"(явно: input_topic:={name})")
+ self._topics.append(name)
+ self.subs.append(self.create_subscription(
+ PointCloud2, name, self._on_cloud, self._qos, raw=self.raw))
+
+ def _on_cloud(self, msg) -> None:
+ self._received += 1
+ if not self.raw:
+ ok, why = ros_conv.has_required_fields(msg)
+ if not ok:
+ self.get_logger().warn(f"кадр пропущен: {why}", throttle_duration_sec=5.0)
+ return
+ if not self.async_worker:
+ # Обработка прямо в колбэке. Такт конвейера втрое короче периода
+ # кадров, поэтому исполнителю ROS есть когда работать, а отдельный
+ # поток здесь только отнимает GIL у приёма: в измерениях он ронял
+ # выработку с 10 до 2 Гц, хотя сам такт оставался 32 мс.
+ # Отбрасывание устаревших кадров при этом делает очередь DDS:
+ # её глубина `queue_depth` и есть «хранить только свежее».
+ t0 = time.perf_counter()
+ try:
+ self._process(msg)
+ except Exception as exc:
+ self.get_logger().error(f"сбой обработки кадра: {exc!r}")
+ self._cycle_ms = (time.perf_counter() - t0) * 1e3
+ return
+ with self._lock:
+ if self._latest is not None:
+ self._dropped += 1
+ self._latest = msg
+ self._wake.set()
+
+ # ------------------------------------------------------------------ обработка
+
+ def _loop(self) -> None:
+ while not self._stop:
+ self._wake.wait(timeout=0.5)
+ self._wake.clear()
+ with self._lock:
+ msg, self._latest = self._latest, None
+ if msg is None:
+ continue
+ t0 = time.perf_counter()
+ try:
+ self._process(msg)
+ except Exception as exc: # нода не должна падать на кадре
+ self.get_logger().error(f"сбой обработки кадра: {exc!r}")
+ # полный такт рабочего потока: конвейер плюс разбор и публикация
+ self._cycle_ms = (time.perf_counter() - t0) * 1e3
+
+ def _process(self, msg) -> None:
+ if self.raw:
+ pc = parse_pointcloud2(msg)
+ header = Header()
+ sec = int(pc.stamp)
+ header.stamp.sec = sec
+ header.stamp.nanosec = int(round((pc.stamp - sec) * 1e9))
+ header.frame_id = pc.frame_id
+ else:
+ pc = ros_conv.from_ros(msg)
+ header = msg.header
+
+ want_view = self.pub_view.get_subscription_count() > 0
+ need_debug = (bool(self.par["publish_debug_cloud"]) or self.brain is not None
+ or want_view)
+ res = self.fg.process(pc, keep_debug=need_debug)
+ if res is None:
+ self.get_logger().info(
+ f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}",
+ throttle_duration_sec=2.0)
+ return
+
+ if self.fg.time_jumps != self._jumps:
+ self._jumps = self.fg.time_jumps
+ self._alarm = False
+ self.get_logger().info("время записи скакнуло (перемотка или повтор) — "
+ "треки и одометрия начаты заново")
+
+ src_frame = header.frame_id or self.fixed_frame
+ self._ensure_tf(src_frame)
+ frame = str(self.par["frame_id"]) or self.fixed_frame
+ d = res.decision
+
+ out = ObstacleStatus()
+ out.header = header
+ out.header.frame_id = frame
+ out.detected = bool(d.detected)
+ out.emergency = bool(d.emergency)
+ out.distance = float(d.distance)
+ out.time_to_collision = float(d.ttc)
+ out.confidence = float(d.confidence)
+ out.speed = float(d.speed)
+ out.stopping_distance = float(d.stopping_distance)
+ out.processing_ms = float(res.total_ms)
+ for o in d.objects:
+ m = DetectedObject()
+ m.distance = float(o.distance); m.lateral = float(o.lateral)
+ m.height = float(o.height); m.width = float(o.width)
+ m.size_v = float(o.size_v); m.confidence = float(o.confidence)
+ m.novelty = float(o.novelty); m.n_rays = int(o.n_rays)
+ m.track_id = int(o.track_id); m.time_to_collision = float(o.ttc)
+ out.objects.append(m)
+ self.pub_status.publish(out)
+ self.pub_flag.publish(Bool(data=bool(d.detected)))
+ self.pub_dist.publish(Float32(data=float(d.distance if d.detected else -1.0)))
+ self._report(d)
+
+ if bool(self.par["publish_markers"]):
+ self.pub_markers.publish(self._markers(d, frame, header.stamp, res.corridor))
+ if want_view and res.tf is not None:
+ self.pub_view.publish(self._view_cloud(res, frame, header.stamp))
+ self._publish_diag(res, header.stamp)
+
+ if bool(self.par["publish_debug_cloud"]):
+ cloud = self._debug_cloud(res, frame, header.stamp)
+ if cloud is not None:
+ self.pub_cloud.publish(cloud)
+
+ # схема мозга рисуется реже кадров лидара: она для человека, не для системы
+ if self.brain is not None and self.pub_brain is not None:
+ now = time.monotonic()
+ if now - self._brain_last >= self.brain_period:
+ self._brain_last = now
+ img = self.brain.render(res)
+ if img is not None:
+ self.pub_brain.publish(self.brain.to_msg(img, header.stamp, frame))
+
+ def _report(self, d) -> None:
+ """Итог в консоль: у стенда результат виден без RViz и `ros2 topic echo`.
+
+ Пишется смена состояния, а пока тревога держится — ближайшая дальность
+ не чаще раза в секунду: построчный вывод на 10 Гц читать невозможно.
+ """
+ if d.detected:
+ self._alarm_frames += 1
+ text = (f"{d.distance:.1f} м, уверенность {d.confidence:.2f}, "
+ f"объектов {len(d.objects)}")
+ if np.isfinite(d.ttc) and d.speed > 0.5:
+ text += f", до столкновения {d.ttc:.1f} с"
+ head = "ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency else "ПРЕПЯТСТВИЕ"
+ if not self._alarm:
+ self.get_logger().warn(f"{head}: {text}")
+ else:
+ self.get_logger().warn(f"{head.lower()}: {text}",
+ throttle_duration_sec=1.0)
+ elif self._alarm:
+ self.get_logger().info("путь свободен")
+ self._alarm = bool(d.detected)
+
+ def _debug_cloud(self, res, frame: str, stamp):
+ """Лучи кандидатов, раскрашенные по новизне, — для наглядности в RViz."""
+ tf = res.tf
+ if tf is None or not res.candidates:
+ return None
+ xs, ys, zs, ws = [], [], [], []
+ for c in res.candidates:
+ rays = c.extra.get("rays")
+ if rays is None:
+ continue
+ ii, jj = rays
+ xs.append(tf.u[ii, jj])
+ ys.append(-tf.d[ii, jj])
+ zs.append(tf.h[ii, jj])
+ ws.append(np.full(ii.size, c.novelty, np.float32))
+ if not xs:
+ return None
+ return self._cloud_msg(np.concatenate(xs), np.concatenate(ys),
+ np.concatenate(zs), np.concatenate(ws), frame, stamp)
+
+ def _view_cloud(self, res, frame: str, stamp):
+ """Облако обзора для RViz: сектор обработки в координатах пути.
+
+ Сырое облако — до 900 тысяч точек и 24 МБ на кадр: RViz на нём тормозит,
+ а в части записей оно ещё и идёт в другой топик, которого конфиг RViz не
+ знает. Здесь только лучи сектора обработки (до 77 тысяч), выровненные по
+ плоскости рельсов: x — поперёк пути, −y — вдоль, z — высота над головкой
+ рельса, как у рамок препятствий, так что рамка стоит ровно на полу.
+ Публикуется, только когда на топик кто-то подписан.
+ """
+ tf = res.tf
+ m = tf.valid & (tf.d > 0.5) & (tf.d < 250.0) & np.isfinite(tf.h)
+ return self._cloud_msg(tf.u[m], -tf.d[m], tf.h[m], tf.inten[m], frame, stamp)
+
+ @staticmethod
+ def _cloud_msg(xs, ys, zs, ws, frame: str, stamp) -> PointCloud2:
+ pts = np.empty(xs.size, dtype=np.dtype([("x", "f4"), ("y", "f4"), ("z", "f4"),
+ ("intensity", "f4")]))
+ pts["x"] = xs
+ pts["y"] = ys
+ pts["z"] = zs
+ pts["intensity"] = ws
+
+ msg = PointCloud2()
+ msg.header.stamp = stamp
+ msg.header.frame_id = frame
+ msg.height = 1
+ msg.width = pts.size
+ msg.fields = [PointField(name=n, offset=o, datatype=PointField.FLOAT32, count=1)
+ for n, o in (("x", 0), ("y", 4), ("z", 8), ("intensity", 12))]
+ msg.is_bigendian = False
+ msg.point_step = 16
+ msg.row_step = 16 * pts.size
+ msg.is_dense = True
+ # array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python —
+ # на облаке обзора это 45 мс на кадр вместо одной (замерено).
+ msg.data = array.array("B", pts.tobytes())
+ return msg
+
+ def _ensure_tf(self, src_frame: str) -> None:
+ if src_frame in self._tf_sent or src_frame == self.fixed_frame:
+ return
+ t = TransformStamped()
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = self.fixed_frame
+ t.child_frame_id = src_frame
+ t.transform.rotation.w = 1.0
+ self._tf.sendTransform(t)
+ self._tf_sent.add(src_frame)
+ self.get_logger().info(f"кадр лидара «{src_frame}» связан с «{self.fixed_frame}»")
+
+ # ------------------------------------------------------------------ визуализация
+
+ def _markers(self, d, frame: str, stamp, corridor=None) -> MarkerArray:
+ arr = MarkerArray()
+ clear = Marker()
+ clear.header.frame_id = frame
+ clear.header.stamp = stamp
+ clear.action = Marker.DELETEALL
+ arr.markers.append(clear)
+
+ for i, o in enumerate(d.objects):
+ m = Marker()
+ m.header.frame_id = frame
+ m.header.stamp = stamp
+ m.ns = "flyguard"
+ m.id = i + 1
+ m.type = Marker.CUBE
+ m.action = Marker.ADD
+ # координаты пути: вперёд = −Y, вправо = +X, вверх = +Z от головки
+ # рельса — те же, что у облака обзора, поэтому рамка стоит на полу.
+ # Поперёк — смещение в системе лидара, а не от оси пути: в кривой
+ # облако не выпрямлено, и рамка по `lateral` встала бы в стороне
+ # от своих точек.
+ x = float(o.sensor_x)
+ m.pose.position.x = x if np.isfinite(x) else float(o.lateral)
+ m.pose.position.y = float(-o.distance)
+ m.pose.position.z = float(o.height + o.size_v / 2)
+ m.pose.orientation.w = 1.0
+ m.scale.x = max(float(o.width), 0.3)
+ m.scale.y = max(float(o.width), 0.3)
+ m.scale.z = max(float(o.size_v), 0.3)
+ hot = float(np.clip(o.confidence, 0.0, 1.0))
+ m.color.r = 1.0
+ m.color.g = float(1.0 - hot)
+ m.color.b = 0.0
+ m.color.a = 0.55
+ arr.markers.append(m)
+
+ txt = Marker()
+ txt.header = m.header
+ txt.ns = "flyguard_text"
+ txt.id = 1000 + i
+ txt.type = Marker.TEXT_VIEW_FACING
+ txt.action = Marker.ADD
+ txt.pose = m.pose
+ txt.pose.position.z += 1.0
+ # 1.4 м: при 0.8 подпись у дальней рамки читалась с трудом
+ txt.scale.z = 1.4
+ txt.color.r = txt.color.g = txt.color.b = txt.color.a = 1.0
+ # латиница: в шрифте RViz нет кириллицы, и «55 м» выходило «55 »
+ txt.text = f"{o.distance:.0f} m p={o.confidence:.2f}"
+ arr.markers.append(txt)
+ arr.markers.extend(self._gauge_markers(frame, stamp, corridor))
+ return arr
+
+ def _gauge_markers(self, frame: str, stamp, corridor=None) -> list[Marker]:
+ """Контур габарита в координатах облака обзора.
+
+ Узел проверяет объединение двух габаритов (`TrackFrame.lateral`):
+ прямого — вдоль оси лидара, и изогнутого — вдоль оценённой оси пути.
+ Облако обзора выровнено по плоскости рельсов, но в кривой не
+ выпрямлено, поэтому в кривой рисуются оба: изогнутый ярко, прямой
+ бледно. На прямом пути они совпадают, и рисуется один. Что внутри
+ оранжевого контура, то узел и проверяет; колонна или шкаф за контуром —
+ не его забота. Рамки поперёк — через 20 м, для глубины.
+ """
+ p = self.fg.p
+ far = min(p.d_max, 200.0)
+ boxes = [(p.half_width, p.h_lo, p.h_hi, p.d_min, far)]
+ if p.h_top > p.h_hi and p.half_width_top > 0.0:
+ boxes.append((p.half_width_top, p.h_hi, p.h_top, p.d_min, min(p.top_d_max, far)))
+ curved = None
+ if corridor is not None and corridor.n_slices > 0:
+ ds = np.arange(p.d_min, far + 1e-6, 5.0, dtype=np.float32)
+ # меньше 15 см контуры сливаются в один — второй незачем
+ if float(np.abs(corridor.centre(ds)).max()) > 0.15:
+ curved = corridor.centre
+ layers = [(None, 0.3 if curved is not None else 0.7)]
+ if curved is not None:
+ layers.append((curved, 0.8))
+ out = []
+ for i, (centre, alpha) in enumerate(layers):
+ m = Marker()
+ m.header.frame_id = frame
+ m.header.stamp = stamp
+ m.ns = "flyguard_gauge"
+ m.id = i
+ m.type = Marker.LINE_LIST
+ m.action = Marker.ADD
+ m.pose.orientation.w = 1.0
+ m.scale.x = 0.04
+ m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.55, 0.0, alpha
+ m.points = [Point(x=float(x), y=float(y), z=float(z))
+ for x, y, z in gauge_outline(boxes, centre)]
+ out.append(m)
+ return out
+
+ def _report_device(self) -> None:
+ """Где идёт счёт — первой строкой журнала: на стенде это видно без RViz."""
+ fg = self.fg
+ if fg.gpu_pending:
+ self.get_logger().info(
+ "вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне "
+ "(в Docker под WSL до 20 с), результат тот же")
+ self._gpu_watch = self.create_timer(0.5, self._watch_gpu)
+ elif fg.gpu_active:
+ self._say_gpu()
+ elif fg.gpu_error:
+ self.get_logger().warn(f"вычисления: процессор — видеокарта не поднялась ({fg.gpu_error})")
+ else:
+ self._say_cpu()
+ self._gpu_on = fg.gpu_active
+
+ def _say_gpu(self, note: str = "") -> None:
+ from .device import get_device_info
+ info = get_device_info(str(self.par["device"]))
+ self.get_logger().info(
+ f"вычисления: видеокарта {info.get('name', '?')} "
+ f"({info.get('total_memory_mb', 0) / 1024:.0f} ГБ, CUDA {info.get('cuda_version')}) — "
+ f"сетчатка, ламина, кластеризация; остальное на процессоре{note}")
+
+ def _say_cpu(self) -> None:
+ why = ("задано device=cpu" if str(self.par["device"]).lower() == "cpu"
+ else "CUDA недоступна: нет видеокарты, драйвера или контейнер запущен без --gpus")
+ self.get_logger().info(f"вычисления: процессор ({why})")
+
+ def _watch_gpu(self) -> None:
+ """Видеокарта поднималась в фоне — сказать в журнал, чем кончилось."""
+ fg = self.fg
+ if fg.gpu_pending:
+ return
+ self._gpu_watch.cancel()
+ self._gpu_on = fg.gpu_active
+ if fg.gpu_active:
+ self._say_gpu(f" (готова через {time.monotonic() - self._t_start:.1f} с после старта)")
+ elif fg.gpu_error:
+ self.get_logger().warn(
+ f"видеокарта не поднялась ({fg.gpu_error}) — считаю на процессоре")
+ else:
+ self._say_cpu()
+
+ def _publish_diag(self, res, stamp) -> None:
+ msg = DiagnosticArray()
+ msg.header.stamp = stamp
+ st = DiagnosticStatus()
+ st.name = "flyguard"
+ st.hardware_id = "lidar"
+ total = res.total_ms
+ st.level = (DiagnosticStatus.OK if total < 90 else DiagnosticStatus.WARN)
+ st.message = f"{total:.1f} мс/кадр"
+ st.values = [KeyValue(key=k, value=f"{v:.2f}") for k, v in res.timings.items()]
+ st.values.append(KeyValue(key="candidates", value=str(len(res.candidates))))
+ st.values.append(KeyValue(key="tracks", value=str(len(self.fg.cx.tracks))))
+ st.values.append(KeyValue(key="dropped_frames", value=str(self._dropped)))
+ # считает сама нода: внешний подписчик тоже теряет сообщения и занижает оценку
+ st.values.append(KeyValue(key="frames_processed", value=str(self.fg.frames_seen)))
+ # Настоящий счётчик приёма, включая кадры, ушедшие на калибровку
+ # решётки: без них разница «принято минус обработано» выглядела
+ # потерей, хотя это цена восстановления геометрии лучей по данным.
+ st.values.append(KeyValue(key="frames_received", value=str(self._received)))
+ st.values.append(KeyValue(key="frames_calibration",
+ value=str(max(self._received - self.fg.frames_seen
+ - self._dropped, 0))))
+ st.values.append(KeyValue(key="cycle_ms", value=f"{self._cycle_ms:.1f}"))
+ st.values.append(KeyValue(key="device", value=self.fg.device))
+ if self._gpu_on and not self.fg.gpu_active:
+ # видеокарта отказала на ходу: кадр досчитан на процессоре, дальше — только он
+ self._gpu_on = False
+ self.get_logger().warn(
+ f"видеокарта отказала ({self.fg.gpu_error}) — дальше считаю на процессоре")
+ if res.ego:
+ st.values.append(KeyValue(key="speed_kmh", value=f"{res.ego.kmh:.1f}"))
+ st.values.append(KeyValue(key="rail_height_m", value=f"{res.plane.height:.3f}"))
+ st.values.append(KeyValue(key="curve_radius_m", value=f"{res.corridor.radius:.0f}"))
+ msg.status.append(st)
+ self.pub_diag.publish(msg)
+
+ def summary(self) -> str:
+ """Итог сеанса: по нему сразу видно, дошли ли кадры и сколько было тревог."""
+ seen = self.fg.frames_seen
+ calib = max(self._received - seen - self._dropped, 0)
+ return (f"итог: принято кадров {self._received}, обработано {seen}, "
+ f"на калибровку {calib}, пропущено {self._dropped}; "
+ f"кадров с тревогой {self._alarm_frames}")
+
+ def destroy_node(self) -> bool:
+ self._stop = True
+ self._wake.set()
+ return super().destroy_node()
+
+
+def main(argv=None) -> None:
+ rclpy.init(args=argv)
+ node = FlyGuardNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ # По Ctrl+C обработчик rclpy успевает закрыть контекст раньше, и журнал
+ # ROS тогда ругается «Failed to publish log message to rosout».
+ if rclpy.ok():
+ node.get_logger().info(node.summary())
+ else:
+ print(f"[flyguard] {node.summary()}", flush=True)
+ node.destroy_node()
+ rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/flyguard/player_keys.py b/flyguard/player_keys.py
new file mode 100644
index 0000000..1fe0d3f
--- /dev/null
+++ b/flyguard/player_keys.py
@@ -0,0 +1,183 @@
+"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
+
+`ros2 bag play`, запущенный из launch (`bag:=...`), клавиатуры не слышит — у
+него нет терминала. Пульт управляет им через сервисы rosbag2 из второго
+терминала того же контейнера:
+
+ docker exec -it flyguard_demo flyguard-keys /data/<запись>
+
+ пробел пауза / продолжить
+ ← → на 3 секунды назад / вперёд
+ . один кадр вперёд (на паузе)
+ ↑ ↓ быстрее / медленнее
+ 0 в начало записи
+ q выйти (запись играет дальше)
+
+Текущее время записи берётся из /clock — launch запускает проигрыватель с
+`--clock`. Путь к записи нужен только для клавиши «0»: из metadata.yaml
+читается время её начала. Узел детектора после перемотки сам сбрасывает треки
+и одометрию (скачок времени, `pipeline.FlyGuard.process`).
+"""
+from __future__ import annotations
+
+import re
+import select
+import sys
+import termios
+import tty
+from pathlib import Path
+
+import rclpy
+from builtin_interfaces.msg import Time
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from rosbag2_interfaces.srv import GetRate, IsPaused, PlayNext, Seek, SetRate, TogglePaused
+from rosgraph_msgs.msg import Clock
+
+STEP_S = 3.0
+RATES = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0]
+PLAYER = "/rosbag2_player/"
+KEYS = """ пробел пауза / продолжить
+ ← → на 3 секунды назад / вперёд
+ . один кадр вперёд (на паузе)
+ ↑ ↓ быстрее / медленнее
+ 0 в начало записи
+ q выйти (запись играет дальше)
+"""
+
+
+def bag_span(path: str | None) -> tuple[float | None, float | None]:
+ """Начало и длительность записи из metadata.yaml, секунды."""
+ if not path:
+ return None, None
+ meta = Path(path) / "metadata.yaml"
+ if not meta.exists():
+ return None, None
+ txt = meta.read_text(encoding="utf-8")
+ s = re.search(r"starting_time:\s*\n\s*nanoseconds_since_epoch:\s*(\d+)", txt)
+ d = re.search(r"duration:\s*\n\s*nanoseconds:\s*(\d+)", txt)
+ return (int(s.group(1)) * 1e-9 if s else None, int(d.group(1)) * 1e-9 if d else None)
+
+
+class PlayerKeys(Node):
+ def __init__(self, start: float | None, duration: float | None):
+ super().__init__("flyguard_player_keys")
+ self.now: float | None = None
+ self.start = start
+ self.duration = duration
+ self.state, self.rate = "идёт", 1.0
+ # /clock проигрыватель шлёт без подтверждений (best effort): с надёжной
+ # подпиской QoS не сходится, и время не приходит вовсе
+ self.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data)
+ self.cli = {name: self.create_client(srv, PLAYER + name) for name, srv in (
+ ("toggle_paused", TogglePaused), ("is_paused", IsPaused), ("seek", Seek),
+ ("set_rate", SetRate), ("get_rate", GetRate), ("play_next", PlayNext))}
+
+ def _on_clock(self, msg: Clock) -> None:
+ self.now = msg.clock.sec + msg.clock.nanosec * 1e-9
+ if self.start is None:
+ self.start = self.now
+
+ def call(self, name: str, req=None):
+ cli = self.cli[name]
+ if not cli.wait_for_service(timeout_sec=2.0):
+ self.say("проигрыватель не отвечает — запись закончилась или запущена без launch?")
+ return None
+ fut = cli.call_async(req if req is not None else cli.srv_type.Request())
+ rclpy.spin_until_future_complete(self, fut, timeout_sec=3.0)
+ return fut.result()
+
+ def seek(self, t: float) -> None:
+ # за конец записи проигрыватель просто закончит её — не пускаем
+ if self.start is not None:
+ t = max(t, self.start)
+ if self.duration:
+ t = min(t, self.start + self.duration - 0.5)
+ req = Seek.Request()
+ sec = int(t)
+ req.time = Time(sec=sec, nanosec=int((t - sec) * 1e9))
+ self.call("seek", req)
+
+ def set_rate(self, up: bool) -> None:
+ got = self.call("get_rate")
+ cur = got.rate if got else 1.0
+ if up:
+ new = next((r for r in RATES if r > cur + 1e-6), RATES[-1])
+ else:
+ new = next((r for r in reversed(RATES) if r < cur - 1e-6), RATES[0])
+ req = SetRate.Request()
+ req.rate = float(new)
+ self.call("set_rate", req)
+
+ def status(self, ask: bool = True) -> str:
+ """Строка состояния; ask=False — без запросов к проигрывателю."""
+ if ask:
+ paused = self.call("is_paused")
+ rate = self.call("get_rate")
+ self.state = "ПАУЗА" if paused and paused.paused else "идёт"
+ self.rate = rate.rate if rate else 1.0
+ pos = (f"{self.now - self.start:6.1f} с" if self.now is not None and self.start
+ else " ? с")
+ return f"{pos} {self.state:6s} скорость ×{self.rate:g}"
+
+ @staticmethod
+ def say(text: str) -> None:
+ sys.stdout.write("\r\033[K" + text)
+ sys.stdout.flush()
+
+
+def read_key(fd: int) -> str:
+ ch = sys.stdin.read(1)
+ if ch != "\x1b":
+ return ch
+ if select.select([fd], [], [], 0.05)[0]:
+ seq = sys.stdin.read(2)
+ return {"[A": "up", "[B": "down", "[C": "right", "[D": "left"}.get(seq, "")
+ return "esc"
+
+
+def main(argv=None) -> None:
+ args = [a for a in (argv or sys.argv[1:]) if not a.startswith("--")]
+ rclpy.init()
+ node = PlayerKeys(*bag_span(args[0] if args else None))
+ print(KEYS)
+ fd = sys.stdin.fileno()
+ old = termios.tcgetattr(fd)
+ try:
+ tty.setcbreak(fd)
+ node.say(node.status())
+ while rclpy.ok():
+ if not select.select([fd], [], [], 0.2)[0]:
+ rclpy.spin_once(node, timeout_sec=0.0)
+ node.say(node.status(ask=False)) # бегущее время записи
+ continue
+ key = read_key(fd)
+ rclpy.spin_once(node, timeout_sec=0.05)
+ if key in ("q", "й", "esc"):
+ break
+ if key == " ":
+ node.call("toggle_paused")
+ elif key in ("left", "right") and node.now is not None:
+ node.seek(node.now + (STEP_S if key == "right" else -STEP_S))
+ elif key in ("left", "right"):
+ node.say("ещё нет времени записи (/clock) — подождите секунду")
+ continue
+ elif key in (".", "ю"):
+ node.call("play_next")
+ elif key in ("up", "down"):
+ node.set_rate(key == "up")
+ elif key == "0" and node.start is not None:
+ node.seek(node.start)
+ rclpy.spin_once(node, timeout_sec=0.1)
+ node.say(node.status())
+ except KeyboardInterrupt:
+ pass
+ finally:
+ termios.tcsetattr(fd, termios.TCSADRAIN, old)
+ print()
+ node.destroy_node()
+ rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/flyguard/ros_conv.py b/flyguard/ros_conv.py
new file mode 100644
index 0000000..9feea83
--- /dev/null
+++ b/flyguard/ros_conv.py
@@ -0,0 +1,38 @@
+"""Преобразование sensor_msgs/PointCloud2 из rclpy во внутреннее представление.
+
+Внутри ноды сообщение уже разобрано транспортом, поэтому CDR-парсер не нужен:
+достаточно посмотреть на поля и построить структурированный numpy-массив
+поверх готового буфера, без копирования.
+"""
+from __future__ import annotations
+
+import numpy as np
+
+from .cdr import PointCloud2, point_dtype
+
+
+def from_ros(msg) -> PointCloud2:
+ """sensor_msgs.msg.PointCloud2 → flyguard.cdr.PointCloud2 (без копирования)."""
+ fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
+ fields.sort(key=lambda f: f[1])
+ dt = point_dtype(fields, msg.point_step)
+ buf = msg.data if isinstance(msg.data, (bytes, bytearray, memoryview)) else \
+ np.asarray(msg.data, np.uint8).tobytes()
+ n = msg.height * msg.width
+ pts = np.frombuffer(buf, dtype=dt, count=n)
+ 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=msg.is_dense, points=pts)
+
+
+def has_required_fields(msg) -> tuple[bool, str]:
+ """Проверить, что в облаке есть всё необходимое конвейеру."""
+ names = {f.name for f in msg.fields}
+ need = {"x", "y", "z", "intensity"}
+ missing = need - names
+ if missing:
+ return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
+ if msg.height * msg.width == 0:
+ return False, "пустое облако"
+ return True, ""
diff --git a/ros2_ws/src/flyguard/config/flyguard.rviz b/ros2_ws/src/flyguard/config/flyguard.rviz
new file mode 100644
index 0000000..3347682
--- /dev/null
+++ b/ros2_ws/src/flyguard/config/flyguard.rviz
@@ -0,0 +1,109 @@
+# Конфиг RViz FlyGuard. Без схемы мозга: трёхмерный вид на всё окно.
+# Раскладку панелей хранит «QMainWindow State» — сериализация Qt, собранная
+# tools/rviz_layout.py; вручную её не править.
+Panels:
+ - Class: rviz_common/Displays
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Препятствия1
+ Splitter Ratio: 0.5
+ Tree Height: 700
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Class: rviz_default_plugins/Grid
+ Name: Сетка
+ Enabled: true
+ Cell Size: 10
+ Plane Cell Count: 40
+ Color: 40; 40; 45
+ Line Style:
+ Line Width: 0.02
+ Value: Lines
+ Reference Frame:
+ # Облако обзора публикует сам узел: сектор обработки, выровненный по рельсам
+ # (z — высота над головкой рельса), до 77 тысяч точек. Сырое облако — до
+ # 900 тысяч точек и 24 МБ на кадр, и топик у записей разный.
+ - Class: rviz_default_plugins/PointCloud2
+ Name: Облако обзора
+ Enabled: true
+ Topic:
+ Value: /flyguard/view_cloud
+ Depth: 2
+ Durability Policy: Volatile
+ Reliability Policy: Best Effort
+ History Policy: Keep Last
+ Style: Points
+ Size (Pixels): 2
+ Color Transformer: AxisColor
+ Axis: Z
+ Autocompute Value Bounds: false
+ Min Value: -0.3
+ Max Value: 5.5
+ Use Fixed Frame: true
+ Decay Time: 0
+ Alpha: 1
+ - Class: rviz_default_plugins/PointCloud2
+ Name: Отладочное облако
+ Enabled: false
+ Topic:
+ Value: /flyguard/debug_cloud
+ Depth: 2
+ Durability Policy: Volatile
+ Reliability Policy: Reliable
+ History Policy: Keep Last
+ Style: Points
+ Size (Pixels): 3
+ Color Transformer: Intensity
+ Alpha: 1
+ - Class: rviz_default_plugins/MarkerArray
+ Name: Препятствия
+ Enabled: true
+ Topic:
+ Value: /flyguard/markers
+ Depth: 5
+ Durability Policy: Volatile
+ Reliability Policy: Reliable
+ History Policy: Keep Last
+ Namespaces: {}
+ Global Options:
+ Background Color: 20; 20; 24
+ Fixed Frame: lidar
+ Frame Rate: 30
+ Tools:
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Name: Вид сверху-сзади
+ Distance: 70
+ Focal Point:
+ X: 0
+ Y: -45
+ Z: 1
+ Pitch: 0.45
+ Yaw: 1.5708
+ Near Clip Distance: 0.05
+ Target Frame:
+ Saved:
+ - Class: rviz_default_plugins/Orbit
+ Name: Обзор пути
+ Distance: 140
+ Focal Point:
+ X: 0
+ Y: -90
+ Z: 1
+ Pitch: 0.25
+ Yaw: 1.5708
+ Target Frame:
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1000
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000001000000000000012c00000384fc0200000001fb000000100044006900730070006c0061007900730100000000000003840000006400ffffff000006280000038400000004000000040000000800000008
+ Width: 1880
diff --git a/ros2_ws/src/flyguard/config/flyguard.yaml b/ros2_ws/src/flyguard/config/flyguard.yaml
new file mode 100644
index 0000000..631ffa9
--- /dev/null
+++ b/ros2_ws/src/flyguard/config/flyguard.yaml
@@ -0,0 +1,126 @@
+# Параметры ноды FlyGuard.
+# Меняются при запуске: ros2 launch flyguard detect.launch.py fov_deg:=35.0
+
+flyguard:
+ ros__parameters:
+ # --- вход -------------------------------------------------------------
+ # В предоставленных записях встречаются оба имени топика, поэтому нода
+ # подписывается и на основной, и на запасные: какой появится, с тем и работает.
+ input_topic: /lidar_points
+ fallback_topics:
+ - /sensing/lidar/hesai128/pointcloud
+ - /points_raw
+ # Кадр лидара весит до 24 МБ и уходит по UDP десятками тысяч фрагментов.
+ # При BEST_EFFORT потеря одного фрагмента отбрасывает всё сообщение целиком,
+ # поэтому по умолчанию надёжная доставка.
+ best_effort: false
+ queue_depth: 10
+ # Сырые байты CDR вместо сборки Python-объекта sensor_msgs: на 24 МБ она
+ # стоит дороже всей обработки. Разбор идёт своим парсером, без копирования.
+ raw_subscription: true
+ # Обработка в колбэке, а не в отдельном потоке: поток боролся за GIL
+ # с приёмом и ронял выработку с 10 до 2 Гц при такте конвейера 32 мс.
+ async_worker: false
+ frame_id: "" # пусто — берётся из заголовка сообщения
+
+ # --- габарит и зона поиска -------------------------------------------
+ # Полуширина габарита: так его задают организаторы в синтетике 24.09
+ # («у края» — до 1.13 м от оси, «вне, но близко» — с 1.14). Было 1.6:
+ # ложная тревога на предмете вне габарита. EXPERIMENTS п. 18.4.
+ half_width: 1.2 # полуширина габарита приближения, м
+ h_lo: 0.28 # нижняя граница над головкой рельса, м
+ h_hi: 2.3 # верхняя граница, м
+ # Верхняя секция: от h_hi до h_top, но уже, |u| < half_width_top, и не
+ # дальше top_d_max — то, что свисает со свода в путь вагона. У стен на этой
+ # высоте кабели и светильники, поэтому основной ширины там нет. 0 — выкл.
+ h_top: 3.3
+ half_width_top: 1.0
+ top_d_max: 90.0
+ # Пол между рельсами (|u| <= 0.85 м) опущен, но только дальше core_from:
+ # лежащий человек высотой 0.30 м при поле 0.28 м виден верхушкой в два
+ # сантиметра. Ближе пол прежний — там в полосу 0.16…0.28 м попадают
+ # головки рельсов, рельс собирается в одну компоненту от кабины, и
+ # предмет выбрасывается вместе с ней. EXPERIMENTS п. 16.3.
+ h_lo_core: 0.16 # пол в колее, м
+ core_from: 30.0 # с какой дальности он опускается, м
+ # Кластеризация ведётся по области выше габарита, чтобы увидеть, что колонна
+ # или рама крепи продолжается до свода: без этого срез конструкции по верхней
+ # границе сам выглядит компактным предметом ростом с габарит.
+ ctx_up: 4.0 # насколько контекст поднимается выше h_hi, м
+ # Разделение фигуры и фона по скорости сближения вдоль луча. Стена,
+ # параллельная движению, вдоль луча не приближается; предмет приближается
+ # ровно на пройденный путь. Поднимает рабочую дальность с 62 до 80 м, но
+ # вчетверо увеличивает ложные тревоги — в тоннеле много поверхностей,
+ # обращённых к поезду. 0 — выключено, рабочее значение 0.6.
+ split_adv: 0.0
+ # Разделение фигуры и фона по контрасту ламины. Гладкая стена даёт нулевой
+ # центр-окружение по построению, предмет на стене — ступеньку. Разрезаются
+ # только компоненты, растёкшиеся вдоль стены (глубже max_depth), и только
+ # по лучам, торчащим из своего окружения дальше этого порога, м.
+ # 0 — выключено.
+ split_gap: 6.0
+ split_near: 55.0 # ближе этого не резать: там предмет виден и так, м
+ # Сколько фигур выносить из одной компоненты. Разрез отрезает фон, и
+ # протяжённость кандидата вдоль пути падает с 6.9 до 0.8 м — вместе с ней
+ # пропадает множитель компактности, давивший конструкции вдесятеро. Поэтому
+ # каждая лишняя фигура дорога: без ограничения 9.4 ложных трека на км, при
+ # двух 7.4, при одной 6.4 — и дальность обнаружения при этом одна и та же.
+ split_top: 1
+
+ # --- привыкание внутри проезда (кратковременная память) -------------
+ # Замысел: гасить формы, встретившиеся в нескольких разных точках пути, без
+ # единого прошлого проезда. ВЫКЛЮЧЕНО: измерено, что избирательности нет —
+ # при неснасыщающейся ёмкости механизм не меняет ничего (7.5 против 7.5
+ # ложных треков на км), а видимый эффект маленькой популяции был насыщением,
+ # которое давит предмет сильнее обстановки. EXPERIMENTS п. 10.
+ enable_habituation: false
+ hab_rate: 0.25 # депрессия KC→MBON за одно новое место
+ hab_place_m: 5.0 # разнос, с которого место считается другим, м
+ hab_recover_m: 800.0 # путь, за который привыкание сходит наполовину, м
+
+ # --- накопление слабых улик (веерное тело) --------------------------
+ # Предмет неподвижен в мире, тоннель проплывает мимо: лучи из габарита
+ # складываются в сетку, привязанную к пути. На дальностях, где локальный
+ # контраст структурно равен нулю (кольцо окружения упирается в стену той
+ # же дальности), это единственная доступная улика. Удваивает рабочую
+ # дальность ценой роста ложных треков с 5.2 до 9.1 на километр.
+ enable_accumulator: true
+ acc_near: 55.0 # ближе этого покадровый тракт справляется сам, м
+ acc_gain: 1.5 # опора, засчитываемая за полный контраст
+ # --- обученное считывание MBON ---------------------------------------
+ # Те же слои грибовидного тела, другой учитель: веса KC→MBON обучены
+ # различать предмет и обстановку на метках, изготовленных физикой
+ # (tools/make_training_set.py). Заменяет ручную формулу веса улики.
+ # Путь подставляется launch-файлом (artifacts/mbon_readout.npz в пакете);
+ # пустой — работает прежняя ручная формула.
+ mbon_path: ""
+ enable_mbon: true
+ # Резкость вероятности: вес наблюдения p**1.5. Ложных на незнакомой линии
+ # 23.5 → 14.7 на км почти без потери дальности (EXPERIMENTS п. 17.4).
+ mbon_power: 1.5
+ mbon_blend: 1.0 # 1 — только модель, 0 — только ручная формула
+ # --- вычисления ----------------------------------------------------------
+ # Сетчатка, ламина и кластеризация — на видеокарте, если PyTorch её видит
+ # (контейнер с --gpus all), иначе и при любом сбое — на процессоре.
+ # Результат одинаков: те же операции в тех же типах (EXPERIMENTS п. 21).
+ # Видеокарта поднимается в фоне: пока греются ядра CUDA, кадры считает процессор.
+ device: auto # auto | cuda | cpu
+ d_min: 4.0 # ближе этого не смотрим, м
+ d_max: 220.0 # дальше физического предела прибора смысла нет, м
+ fov_deg: 30.0 # полусектор обработки по азимуту, °
+ min_rays: 4 # минимум лучей на кандидата
+
+ # --- память тоннеля ---------------------------------------------------
+ # Без неё штатные конструкции тоннеля не подавляются и ложных тревог заметно больше.
+ memory_path: "" # путь подставляется launch-файлом
+
+ # --- вывод ------------------------------------------------------------
+ publish_markers: true
+ publish_debug_cloud: false
+ brain_view: false # вид мозга: для демонстрации, не для замеров задержки
+ # scheme — нарисованная схема нейропилей;
+ # cloud — 139 255 нейронов коннектома FlyWire на своих местах;
+ # hybrid — панели решётки сверху и облако нейронов снизу (по умолчанию).
+ brain_style: hybrid
+ brain_period: 0.2
+ brain_scale: 1 # масштаб вида мозга: 2–3 — картинка для 2K/4K (атласы x2, x3)
diff --git a/ros2_ws/src/flyguard/config/flyguard_brain.rviz b/ros2_ws/src/flyguard/config/flyguard_brain.rviz
new file mode 100644
index 0000000..346d1c9
--- /dev/null
+++ b/ros2_ws/src/flyguard/config/flyguard_brain.rviz
@@ -0,0 +1,124 @@
+# Конфиг RViz FlyGuard. Со схемой мозга: она справа во всю высоту, панель Displays
+# свёрнута (открывается стрелкой у левого края окна).
+# Раскладку панелей хранит «QMainWindow State» — сериализация Qt, собранная
+# tools/rviz_layout.py; вручную её не править.
+Panels:
+ - Class: rviz_common/Displays
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Препятствия1
+ Splitter Ratio: 0.5
+ Tree Height: 700
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Class: rviz_default_plugins/Grid
+ Name: Сетка
+ Enabled: true
+ Cell Size: 10
+ Plane Cell Count: 40
+ Color: 40; 40; 45
+ Line Style:
+ Line Width: 0.02
+ Value: Lines
+ Reference Frame:
+ # Облако обзора публикует сам узел: сектор обработки, выровненный по рельсам
+ # (z — высота над головкой рельса), до 77 тысяч точек. Сырое облако — до
+ # 900 тысяч точек и 24 МБ на кадр, и топик у записей разный.
+ - Class: rviz_default_plugins/PointCloud2
+ Name: Облако обзора
+ Enabled: true
+ Topic:
+ Value: /flyguard/view_cloud
+ Depth: 2
+ Durability Policy: Volatile
+ Reliability Policy: Best Effort
+ History Policy: Keep Last
+ Style: Points
+ Size (Pixels): 2
+ Color Transformer: AxisColor
+ Axis: Z
+ Autocompute Value Bounds: false
+ Min Value: -0.3
+ Max Value: 5.5
+ Use Fixed Frame: true
+ Decay Time: 0
+ Alpha: 1
+ - Class: rviz_default_plugins/PointCloud2
+ Name: Отладочное облако
+ Enabled: false
+ Topic:
+ Value: /flyguard/debug_cloud
+ Depth: 2
+ Durability Policy: Volatile
+ Reliability Policy: Reliable
+ History Policy: Keep Last
+ Style: Points
+ Size (Pixels): 3
+ Color Transformer: Intensity
+ Alpha: 1
+ - Class: rviz_default_plugins/MarkerArray
+ Name: Препятствия
+ Enabled: true
+ Topic:
+ Value: /flyguard/markers
+ Depth: 5
+ Durability Policy: Volatile
+ Reliability Policy: Reliable
+ History Policy: Keep Last
+ Namespaces: {}
+ - Class: rviz_default_plugins/Image
+ Name: Мозг мухи
+ Enabled: true
+ Topic:
+ Value: /flyguard/brain
+ Depth: 2
+ Durability Policy: Volatile
+ Reliability Policy: Reliable
+ History Policy: Keep Last
+ Normalize Range: false
+ Max Value: 255
+ Min Value: 0
+ Global Options:
+ Background Color: 20; 20; 24
+ Fixed Frame: lidar
+ Frame Rate: 30
+ Tools:
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Name: Вид сверху-сзади
+ Distance: 70
+ Focal Point:
+ X: 0
+ Y: -45
+ Z: 1
+ Pitch: 0.45
+ Yaw: 1.5708
+ Near Clip Distance: 0.05
+ Target Frame:
+ Saved:
+ - Class: rviz_default_plugins/Orbit
+ Name: Обзор пути
+ Distance: 140
+ Focal Point:
+ X: 0
+ Y: -90
+ Z: 1
+ Pitch: 0.25
+ Yaw: 1.5708
+ Target Frame:
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1000
+ Hide Left Dock: true
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000002000000000000011800000384fc0200000001fb000000100044006900730070006c0061007900730100000000000003840000006400ffffff000000010000033400000384fc0200000001fb00000012041c043e043704330020043c0443044504380100000000000003840000006400ffffff000003040000038400000004000000040000000800000008
+ Width: 1880
+ Мозг мухи:
+ collapsed: false
diff --git a/ros2_ws/src/flyguard/flyguard/__init__.py b/ros2_ws/src/flyguard/flyguard/__init__.py
new file mode 100644
index 0000000..b6087c0
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/__init__.py
@@ -0,0 +1,16 @@
+"""FlyGuard — обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара.
+
+Архитектура повторяет вычислительные схемы зрительной системы и грибовидных тел
+Drosophila melanogaster, взятые из коннектома (FlyWire / hemibrain):
+
+ retina омматидиальная решётка → дальностный образ
+ stabilizer жужжальца / оцеллии → стабилизация «взгляда»
+ lamina L1/L2, ON/OFF, center-surround→ локальный контраст
+ medulla T4/T5, EMD-корреляторы → оптический поток
+ lobula_plate LPTC (HS/VS), LPLC2 → эго-движение, looming
+ mushroom_body KC + APL + MBON → новизна / знакомость
+ central_complex кольцевой аттрактор (EB) → накопление улик, треки
+ descending Giant Fiber, DNp → решение
+"""
+
+__version__ = "1.0.0"
diff --git a/ros2_ws/src/flyguard/flyguard/bag.py b/ros2_ws/src/flyguard/flyguard/bag.py
new file mode 100644
index 0000000..d99c943
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/bag.py
@@ -0,0 +1,149 @@
+"""Чтение rosbag2 (storage sqlite3) без установленного ROS.
+
+Поддерживает многошардовые бэги (`new_data` — 221 файл `*.db3`), произвольные
+имена топиков и порядок шардов по числовому суффиксу. Метаданные `metadata.yaml`
+не требуются: список топиков берётся из самой БД, что снимает зависимость от PyYAML.
+"""
+from __future__ import annotations
+
+import logging
+import re
+import sqlite3
+from contextlib import closing
+from dataclasses import dataclass
+from pathlib import Path
+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"
+
+
+@dataclass(frozen=True)
+class BagTopic:
+ name: str
+ type: str
+ count: int
+
+
+class Bag:
+ """Последовательное чтение облаков точек из rosbag2."""
+
+ def __init__(self, path: str | Path, topic: str | None = None):
+ path = Path(path)
+ if path.is_dir():
+ shards = sorted(path.glob("*.db3"), key=self._shard_key)
+ elif path.suffix == ".db3":
+ shards = [path]
+ else:
+ raise FileNotFoundError(f"не бэг и не .db3: {path}")
+ if not shards:
+ raise FileNotFoundError(f"в {path} нет файлов *.db3")
+
+ self.path = path
+ 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]:
+ m = _SHARD_RE.search(p.name)
+ return (int(m.group(1)) if m else 0, p.name)
+
+ def _scan_topics(self) -> dict[str, BagTopic]:
+ found: dict[str, BagTopic] = {}
+ for shard in self.shards:
+ with closing(self._connect(shard)) as con:
+ rows = con.execute(
+ "SELECT t.name, t.type, count(m.id) FROM topics t "
+ "LEFT JOIN messages m ON m.topic_id = t.id GROUP BY t.id"
+ ).fetchall()
+ for name, type_, count in rows:
+ prev = found.get(name)
+ found[name] = BagTopic(name, type_, (prev.count if prev else 0) + count)
+ return found
+
+ def _pick_topic(self) -> str:
+ clouds = [t for t in self.topics.values() if t.type == _POINTCLOUD_TYPE]
+ if not clouds:
+ raise ValueError(f"в {self.path} нет топиков {_POINTCLOUD_TYPE}: "
+ f"{sorted(self.topics)}")
+ # при нескольких облачных топиках берём самый наполненный
+ return max(clouds, key=lambda t: t.count).name
+
+ @staticmethod
+ def _connect(shard: Path) -> sqlite3.Connection:
+ return sqlite3.connect(f"file:{shard.as_posix()}?mode=ro&immutable=1", uri=True)
+
+ def __len__(self) -> int:
+ return self.topics[self.topic].count
+
+ def frames(self, start: int = 0, stop: int | None = None,
+ stride: int = 1) -> Iterator[tuple[int, PointCloud2]]:
+ """Выдать (timestamp_ns, облако) для сообщений выбранного топика.
+
+ Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри
+ шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать
+ (битый CDR у реального сенсора), пропускается с предупреждением в лог,
+ а не останавливает чтение бэга — `start`/`stop` считают по позиции
+ сообщения в бэге, а не по числу успешно разобранных.
+ """
+ idx = 0
+ self.n_frames_failed = 0
+ for shard in self.shards:
+ # closing(), а не сам connection: у sqlite3 `with` управляет
+ # транзакцией и файл остаётся открытым — на Windows его потом
+ # невозможно удалить
+ with closing(self._connect(shard)) as con:
+ row = con.execute("SELECT id FROM topics WHERE name = ?",
+ (self.topic,)).fetchone()
+ if row is None:
+ continue
+ topic_id = row[0]
+ n_here = con.execute(
+ "SELECT count(*) FROM messages WHERE topic_id = ?", (topic_id,)
+ ).fetchone()[0]
+ if stop is not None and idx >= stop:
+ return
+ if idx + n_here <= start:
+ idx += n_here
+ continue
+ cur = con.execute(
+ "SELECT timestamp, data FROM messages WHERE topic_id = ? "
+ "ORDER BY timestamp", (topic_id,))
+ for ts, blob in cur:
+ if stop is not None and idx >= stop:
+ return
+ if idx >= start and (idx - start) % stride == 0:
+ 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:
+ lines = [f"бэг: {self.path}",
+ f"шардов: {len(self.shards)}",
+ f"топик: {self.topic}"]
+ for t in sorted(self.topics.values(), key=lambda t: -t.count):
+ mark = "*" if t.name == self.topic else " "
+ lines.append(f" {mark} {t.name} [{t.type}] {t.count} сообщений")
+ return "\n".join(lines)
+
+
+def find_bags(root: str | Path) -> list[Path]:
+ """Найти все каталоги-бэги под указанным корнем."""
+ root = Path(root)
+ if not root.exists():
+ return []
+ out = {p.parent for p in root.rglob("*.db3")}
+ return sorted(out)
diff --git a/ros2_ws/src/flyguard/flyguard/brain_atlas.py b/ros2_ws/src/flyguard/flyguard/brain_atlas.py
new file mode 100644
index 0000000..5337c41
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/brain_atlas.py
@@ -0,0 +1,364 @@
+"""Мозг как облако из 139 255 нейронов, подсвеченное живой активностью.
+
+Отличие от `brain_view` — не в красоте, а в том, чем нарисовано. Там схема,
+нарисованная руками; здесь каждая точка — **настоящий нейрон из коннектома**
+FlyWire FAFB v783, стоящий на своём анатомическом месте, а его яркость берётся
+от той стадии FlyGuard, которой он соответствует.
+
+Симуляции нет и не нужно. Мембранные потенциалы 139 тысяч клеток никто не
+интегрирует: коннектом даёт анатомию и принадлежность клеток, конвейер даёт
+активность по стадиям, а здесь одно накладывается на другое. Стоимость кадра —
+взвешенная сумма десяти заранее посчитанных карт плотности.
+
+Привязка держится на именах типов клеток, а не на догадках. FlyGuard с самого
+начала собран из конкретных схем, и все они в выгрузке есть поимённо:
+
+ LC11 127 нейронов детектор мелкого объекта → lobula.py
+ LPLC2 210 детектор надвигания → medulla.py
+ HS/VS 22 широкопольный поток → medulla.py
+ T4/T5 12245 элементарные детекторы движения
+ KC 5177 клетки Кеньона, разрежённый код → mushroom_body.py
+ MBON 96 выход памяти, новизна
+ APL 2 глобальное торможение
+ DNp01 2 гигантское волокно, экстренное решение → descending.py
+
+Атлас собирается `tools/build_brain_atlas.py` и лежит в `data/brain_atlas.npz`
+(605 КБ). Данные FlyWire — CC-BY 4.0, Dorkenwald et al. и Schlegel et al.,
+Nature 2024.
+"""
+from __future__ import annotations
+
+from collections import deque
+from pathlib import Path
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError:
+ cv2 = None
+
+from .brain_view import BG, INK, _colour, put_text, text_size
+
+ATLAS = Path(__file__).with_name("data") / "brain_atlas.npz"
+
+# Какая стадия конвейера освещает какую группу нейронов. Ключи справа — из
+# `BrainView.activity`; так обе картинки всегда показывают одно и то же.
+STAGE_SOURCE: dict[str, tuple[str, ...]] = {
+ "retina": ("retina",),
+ "lamina": ("lamina",),
+ "medulla": ("medulla",),
+ "lptc": ("lobula_plate",),
+ "looming": ("looming",),
+ "lobula": ("lobula",),
+ "mushroom": ("mb_lobes", "mb_calyx"),
+ "central": ("eb", "pb", "fb"),
+ "descending": ("dn",),
+ "other": (),
+}
+OTHER_BASE = 0.045 # остальной мозг: виден, но не отвлекает
+TRACE_N = 100 # длина ритмограммы: 10 с при 10 Гц
+
+RU = {
+ "retina": "сетчатка R1–R8",
+ "lamina": "ламина L1 / L2",
+ "medulla": "медулла T4 / T5",
+ "lptc": "лоб. пластинка HS / VS",
+ "looming": "LPLC2 надвигание",
+ "lobula": "лобула LC11",
+ "mushroom": "грибовидное тело KC → MBON",
+ "central": "центральный комплекс",
+ "descending": "нисходящие DNp01",
+}
+
+
+class NeuronCloud:
+ """Облако нейронов в анатомических координатах, яркость — активность стадии."""
+
+ def __init__(self, labels: bool = True, gain: float = 1.0, scale: int = 1):
+ self.enabled = cv2 is not None and ATLAS.exists()
+ self.labels = labels
+ self.gain = gain
+ # Масштаб отрисовки: 1 — для узла (1180 × 620), 2–3 — для экрана и
+ # видео в 2K/4K. Сомы берутся из атласа своего размера
+ # (brain_atlas_x2.npz …) — это настоящие координаты, а не растянутая
+ # картинка; шрифты, линии и отступы растут вместе с масштабом.
+ self.s = max(1, int(scale))
+ self._smooth: dict[str, float] = {}
+ self._trace: deque = deque(maxlen=TRACE_N)
+ if not self.enabled:
+ return
+
+ path = ATLAS.with_name(f"brain_atlas_x{self.s}.npz") if self.s > 1 else ATLAS
+ d = np.load(path if path.exists() else ATLAS, allow_pickle=False)
+ self.h, self.w = (int(v) for v in d["shape"])
+ stages = [str(s) for s in d["stages"]]
+ px, py = d["px"].astype(np.int64), d["py"].astype(np.int64)
+ stage, side, depth = d["stage"], d["side"], d["depth"]
+ if not path.exists(): # атласа нужного размера нет — растянуть обычный
+ px, py, self.h, self.w = px * self.s, py * self.s, self.h * self.s, self.w * self.s
+
+ ok = (px >= 0) & (px < self.w) & (py >= 0) & (py < self.h)
+ flat = py * self.w + px
+ # Глубина: дальние сомы тусклее, иначе фронтальная проекция выглядит плоской
+ shade = (0.55 + 0.45 * (1.0 - depth / 255.0)).astype(np.float32)
+
+ # Сомы занимают шестую часть холста; считать по всему полотну незачем.
+ # Карты сразу строятся по занятым пикселям: это убирает из каждого
+ # кадра 80 % работы, а в 4K — ещё и сотни мегабайт промежуточных карт.
+ self.support = np.unique(flat[ok])
+ at = np.searchsorted(self.support, flat)
+
+ # Две карты на стадию — левое и правое полушарие: так видно, с какой
+ # стороны пришёл объект, ровно как у мухи с её двумя оптическими долями.
+ self.keys: list[str] = []
+ maps = []
+ for i, name in enumerate(stages):
+ for sd, tag in ((0, "L"), (1, "R"), (2, "C")):
+ m = ok & (stage == i) & (side == sd)
+ if m.sum() == 0:
+ continue
+ self.keys.append(f"{name}|{tag}")
+ maps.append(np.bincount(at[m], weights=shade[m],
+ minlength=self.support.size).astype(np.float32))
+ self.maps = np.ascontiguousarray(np.stack(maps)) # (K, занятые пиксели)
+ self.stage_of = [k.split("|")[0] for k in self.keys]
+ self.side_of = [k.split("|")[1] for k in self.keys]
+
+ # Якоря подписей — центры тяжести стадии в своём полушарии. Общий
+ # центр тяжести у всех стадий приходится на середину мозга и ничего
+ # не показывает: стадии парные.
+ self.anchor: dict[str, tuple[tuple[int, int], tuple[int, int]]] = {}
+ for i, name in enumerate(stages):
+ if name not in RU:
+ continue
+ a = []
+ for sd in (0, 1):
+ m = ok & (stage == i) & (side == sd)
+ m = m if m.sum() > 30 else (ok & (stage == i))
+ a.append((int(px[m].mean()), int(py[m].mean())))
+ self.anchor[name] = tuple(a)
+
+ self._lut = self._build_lut()
+ self._base = self._draw_base()
+
+ # ------------------------------------------------------------------ статика
+
+ @staticmethod
+ def _build_lut() -> np.ndarray:
+ """Палитра «покой → работа → тревога», та же, что у схемы."""
+ lut = np.zeros((256, 3), np.uint8)
+ for i in range(256):
+ lut[i] = _colour(i / 255.0)
+ return lut
+
+ def _draw_base(self) -> np.ndarray:
+ s = self.s
+ img = np.full((self.h, self.w, 3), BG, np.uint8)
+ put_text(img, "FlyGuard — 139 255 нейронов коннектома FlyWire, "
+ "подсвеченных активностью конвейера",
+ (26 * s, 34 * s), 0.6 * s, (205, 205, 215))
+ cv2.line(img, (26 * s, 46 * s), (self.w - 26 * s, 46 * s), (52, 52, 62), s)
+ return img
+
+ # ------------------------------------------------------------------ кадр
+
+ def weights(self, act: dict[str, float], bias: float) -> np.ndarray:
+ """Яркость каждой полукарты. `bias` < 0 — объект слева, > 0 — справа."""
+ w = np.empty(len(self.keys), np.float32)
+ for i, (stage, side) in enumerate(zip(self.stage_of, self.side_of)):
+ src = STAGE_SOURCE.get(stage, ())
+ v = max((self._smooth.get(k, 0.0) for k in src), default=0.0)
+ v = OTHER_BASE if not src else max(v, OTHER_BASE * 0.7)
+ if side in ("L", "R") and stage in ("retina", "lamina", "medulla",
+ "lptc", "looming", "lobula"):
+ # У мухи зрительный мир перекрещен: предмет справа сильнее
+ # освещает левую долю. Для демонстрации важно, что асимметрия
+ # видна и соответствует стороне объекта.
+ v *= 1.0 + 0.10 * (bias if side == "L" else -bias)
+ w[i] = np.clip(v, 0.0, 1.0)
+ return w
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ from .brain_view import BrainView
+
+ for k, v in BrainView.activity(res).items():
+ self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
+
+ # Ритмограмма: у дрозофилы ритмы мозга меряют электродом, из наших
+ # данных они не выводятся. Поэтому здесь честный след собственного
+ # состояния — сколько работает зрительный тракт, сколько памяти и
+ # когда сработало решение.
+ vis = float(np.mean([self._smooth.get(k, 0.0) for k in
+ ("lamina", "medulla", "lobula", "lobula_plate")]))
+ self._trace.append((float(vis),
+ float(self._smooth.get("dopamine", 0.0)),
+ bool(res.decision.detected),
+ float(res.total_ms),
+ float(self._smooth.get("habituation", 0.0))))
+
+ lat = [c.u for c in res.candidates] or [0.0]
+ bias = float(np.clip(np.mean(lat) / 1.6, -1.0, 1.0))
+
+ vals = self.weights(self.activity_smoothed(), bias) @ self.maps
+ img = self._base.copy()
+ if self.s == 1:
+ idx = np.clip(vals * (255.0 * self.gain), 0, 255).astype(np.uint8)
+ img.reshape(-1, 3)[self.support] = self._lut[idx]
+ # Точки одиночные и на большом холсте теряются: лёгкое размытие делает
+ # облако читаемым, не превращая его в заливку.
+ cv2.GaussianBlur(img, (3, 3), 0, dst=img)
+ else:
+ self._paint_fine(img, vals)
+
+ if self.labels:
+ self._draw_labels(img)
+ self._draw_rhythm(img)
+ self._draw_readout(img, res)
+ return img
+
+ def _paint_fine(self, img, vals) -> None:
+ """Облако в высоком разрешении: каждая сома на своём месте.
+
+ В масштабе 1 пиксель холста собирает несколько сом, и яркость — их
+ сумма. Здесь сома — отдельная точка, поэтому сумма берётся по
+ окрестности той же площади (гауссово окно, нормированное так, что
+ одиночная сома сохраняет свою яркость), а края точек смешиваются с
+ фоном по доле покрытия. Облако выглядит как в узле, только чётче.
+ """
+ sig = 0.55 * self.s
+ norm = 2.0 * np.pi * sig * sig
+ field = np.zeros((2, self.h * self.w), np.float32)
+ field[0, self.support] = vals
+ field[1, self.support] = 1.0
+ v = cv2.GaussianBlur(field[0].reshape(self.h, self.w), (0, 0), sig) * norm
+ a = np.clip(cv2.GaussianBlur(field[1].reshape(self.h, self.w), (0, 0), sig)
+ * norm, 0.0, 1.0)
+ idx = np.clip(v * (255.0 * self.gain), 0, 255).astype(np.uint8)
+ col = self._lut[idx]
+ img[:] = cv2.blendLinear(col, img, a, 1.0 - a)
+
+ def activity_smoothed(self) -> dict[str, float]:
+ return self._smooth
+
+ def _draw_labels(self, img) -> None:
+ """Подписи столбиком у ближнего края с выноской к центру тяжести стадии.
+
+ Центры тяжести половины стадий лежат в середине мозга и налезают друг
+ на друга, поэтому подписи разводятся по вертикали, а привязка остаётся
+ честной — линия ведёт ровно в центр тяжести.
+ """
+ # Зрительный путь подписывается слева, память и решение — справа:
+ # так выноски не пересекают мозг и идут в своё же полушарие.
+ s = self.s
+ left_side = ("retina", "lamina", "medulla", "lptc")
+ cols = {True: 76 * s, False: 76 * s}
+ for name in ("retina", "lamina", "medulla", "lptc",
+ "looming", "lobula", "mushroom", "central", "descending"):
+ if name not in self.anchor:
+ continue
+ left = name in left_side
+ ax, ay = self.anchor[name][0 if left else 1]
+ txt = RU[name]
+ tw, th = text_size(txt, 0.42 * s)
+ x = 26 * s if left else self.w - tw - 26 * s
+ y = cols[left]
+ cols[left] += 27 * s
+ box = img[max(y - th - 5 * s, 0):y + 5 * s, max(x - 6 * s, 0):x + tw + 6 * s]
+ if box.size:
+ box[:] = (box * 0.2).astype(np.uint8)
+ put_text(img, txt, (x, y), 0.42 * s, INK)
+ tip = (x + tw + 7 * s, y - th // 2) if left else (x - 7 * s, y - th // 2)
+ cv2.line(img, tip, (ax, ay), (82, 80, 96), s, cv2.LINE_AA)
+ cv2.circle(img, (ax, ay), 3 * s, (162, 160, 178), s, cv2.LINE_AA)
+
+ def _draw_rhythm(self, img) -> None:
+ """Ритмограмма: след активности и дофамина за последние десять секунд.
+
+ Это не ритм мозга дрозофилы — тот меряют электродом, и из лидарных
+ данных он не выводится. Это след собственного состояния системы:
+ зрительный тракт, сигнал обучения и отметки моментов решения.
+ """
+ if len(self._trace) < 3:
+ return
+ s = self.s
+ x0, y0, w, hgt = 250 * s, self.h - 96 * s, 380 * s, 56 * s
+ cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (40, 39, 48), -1)
+ cv2.rectangle(img, (x0, y0), (x0 + w, y0 + hgt), (60, 58, 70), s)
+ put_text(img, "ритм · 10 с", (x0 + 4 * s, y0 - 6 * s), 0.37 * s, (128, 126, 140))
+
+ n = len(self._trace)
+ xs = x0 + (np.arange(n) * (w - 6 * s) / max(TRACE_N - 1, 1)).astype(int) + 3 * s
+ tr = np.array([(a, b, float(c), e) for a, b, c, _, e in self._trace],
+ np.float32)
+
+ lx = x0 + 78 * s
+ traces = [(0, (176, 196, 92), "зрение"), (1, (86, 178, 240), "дофамин")]
+ if tr[:, 3].max() > 0.0:
+ traces.append((3, (150, 120, 226), "привыкание"))
+ for col, colour, name in traces:
+ ys = (y0 + hgt - 8 * s - tr[:, col] * (hgt - 16 * s)).astype(int)
+ cv2.polylines(img, [np.stack([xs, ys], 1)], False, colour, s, cv2.LINE_AA)
+ cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), colour, 2 * s,
+ cv2.LINE_AA)
+ put_text(img, name, (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
+ lx += 15 * s + text_size(name, 0.35 * s)[0] + 16 * s
+
+ # Полоска решений внизу: где система говорила «препятствие».
+ hot = tr[:, 2] > 0.5
+ if hot.any():
+ for i in np.flatnonzero(hot):
+ cv2.line(img, (int(xs[i]), y0 + hgt - 5 * s), (int(xs[i]), y0 + hgt - 2 * s),
+ (66, 66, 248), s)
+ cv2.line(img, (lx, y0 - 10 * s), (lx + 11 * s, y0 - 10 * s), (66, 66, 248), 2 * s)
+ put_text(img, "решение", (lx + 15 * s, y0 - 6 * s), 0.35 * s, (132, 130, 144))
+
+ def _draw_readout(self, img, res) -> None:
+ d = res.decision
+ hab = float(getattr(res, "hab", 0.0))
+ rows = [("скорость", f"{abs(d.speed) * 3.6:.1f} км/ч"),
+ ("кандидатов", f"{len(res.candidates)}"),
+ ("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
+ ("дофамин PPL1/PAM", f"{self._smooth.get('dopamine', 0.0):.2f}")]
+ # привыкание по умолчанию выключено — строка появляется, только если оно
+ # включено и уже что-то накопило
+ if hab > 0.0:
+ rows.append(("привыкание", f"{hab:.2f}"))
+ rows.append(("обработка", f"{res.total_ms:.1f} мс"))
+ s = self.s
+ y0 = self.h - 126 * s - 23 * s * (len(rows) - 5)
+ # Столбик лежит поверх облака, и серые подписи в нём тонули в точках
+ # оптической доли — подложка, как под выносками стадий.
+ vw = max(text_size(v, 0.46 * s)[0] for _, v in rows)
+ right = min(196 * s + vw + 10 * s, 244 * s) # не заходить на ритмограмму
+ box = img[y0 - 18 * s:y0 + 23 * s * (len(rows) - 1) + 8 * s, 18 * s:right]
+ box[:] = (box * 0.25).astype(np.uint8)
+ for i, (k, v) in enumerate(rows):
+ put_text(img, k, (28 * s, y0 + i * 23 * s), 0.44 * s, (118, 116, 128))
+ put_text(img, v, (196 * s, y0 + i * 23 * s), 0.46 * s, (206, 206, 216))
+
+ if d.detected:
+ txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
+ sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
+ else f"уверенность {d.confidence:.2f}")
+ colour = (66, 66, 248) if d.emergency else (72, 196, 240)
+ else:
+ txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
+ put_text(img, txt, (self.w - 470 * s, self.h - 62 * s), 0.9 * s, colour, 2)
+ if sub:
+ put_text(img, sub, (self.w - 470 * s, self.h - 32 * s), 0.54 * s, colour)
+
+ x = self.w - 300 * s
+ for lvl, name in ((0.10, "покой"), (0.45, "работа"), (0.85, "тревога")):
+ cv2.circle(img, (x, 34 * s), 5 * s, _colour(lvl), -1, cv2.LINE_AA)
+ put_text(img, name, (x + 12 * s, 39 * s), 0.4 * s, (150, 148, 162))
+ x += 14 * s + text_size(name, 0.4 * s)[0] + 22 * s
+
+ # ------------------------------------------------------------------ ROS
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from .brain_view import BrainView
+ return BrainView.to_msg(img, stamp, frame_id)
diff --git a/ros2_ws/src/flyguard/flyguard/brain_hybrid.py b/ros2_ws/src/flyguard/flyguard/brain_hybrid.py
new file mode 100644
index 0000000..9ddfc02
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/brain_hybrid.py
@@ -0,0 +1,145 @@
+"""Гибридный вид: что видит глаз — и что от этого происходит в мозге.
+
+Сверху четыре панели ретинотопической решётки, как их видит муха: дальностный
+образ, ON-канал ламины (что ближе окружения), OFF-канал (что дальше) и
+выделенные кандидаты. Снизу — те же 139 255 нейронов коннектома, подсвеченные
+активностью соответствующих стадий.
+
+Смысл именно в паре: видно не только «мозг светится», но и от чего. Контраст
+в ламине появляется на панели — и в ту же секунду загорается полоса ламины в
+облаке; предмет попадает в габарит — вспыхивает лобула; трек набирает улику —
+разгорается центральный комплекс и уходит вниз к гигантскому волокну.
+
+Стоит около 12 мс и включается флагом, чтобы не попадать в замеры задержки.
+"""
+from __future__ import annotations
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError:
+ cv2 = None
+
+from .brain_atlas import NeuronCloud
+from .brain_view import BG, put_text
+
+PANEL_H = 132 # высота строки панелей, px
+PAD = 10
+
+
+def _norm(a: np.ndarray, lo: float, hi: float) -> np.ndarray:
+ return np.clip((a - lo) / max(hi - lo, 1e-6), 0.0, 1.0)
+
+
+class BrainHybrid:
+ """Панели решётки + облако нейронов в одном кадре."""
+
+ def __init__(self, labels: bool = True, scale: int = 1):
+ self.cloud = NeuronCloud(labels=labels, scale=scale)
+ self.s = self.cloud.s
+ self.enabled = cv2 is not None and self.cloud.enabled
+ if not self.enabled:
+ return
+ self.w = self.cloud.w
+ self.strip_h = (PANEL_H + 34) * self.s
+ self.h = self.cloud.h + self.strip_h
+
+ # ------------------------------------------------------------------ панели
+
+ def _panel(self, img: np.ndarray, title: str, hint: str, slot: int,
+ out: np.ndarray) -> None:
+ n, s = 4, self.s
+ pad, ph, top = PAD * s, PANEL_H * s, 28 * s
+ pw = (self.w - pad * (n + 1)) // n
+ x = pad + slot * (pw + pad)
+ # INTER_AREA, а не NEAREST: столбцы чередуются по эхам, и при
+ # прореживании соседями решётка превращается в шахматку.
+ small = cv2.resize(img, (pw, ph), interpolation=cv2.INTER_AREA)
+ out[top:top + ph, x:x + pw] = small
+ cv2.rectangle(out, (x, top), (x + pw, top + ph), (58, 56, 68), s)
+ from .brain_view import text_size
+ put_text(out, title, (x + 2 * s, 22 * s), 0.4 * s, (188, 188, 200))
+ if hint:
+ tw = text_size(hint, 0.36 * s)[0]
+ # подпись справа рисуется только если не налезает на заголовок
+ if x + pw - tw - 8 * s > x + 2 * s + text_size(title, 0.4 * s)[0]:
+ put_text(out, hint, (x + pw - tw - 2 * s, 22 * s), 0.36 * s, (112, 110, 124))
+
+ def _strip(self, res) -> np.ndarray:
+ out = np.full((self.strip_h, self.w, 3), BG, np.uint8)
+ tf, lam = res.tf, res.lam
+ if tf is None or lam is None:
+ put_text(out, "панели решётки доступны только с keep_debug=True",
+ (PAD * self.s, 24 * self.s), 0.44 * self.s, (140, 138, 152))
+ return out
+
+ # 1. Дальностный образ: ближнее ярче, «нет эха» — ровный тёмный фон,
+ # иначе пустые лучи красятся палитрой и спорят с данными.
+ r = np.where(tf.valid, tf.r, np.nan)
+ depth = 1.0 - _norm(np.nan_to_num(r, nan=220.0), 4.0, 140.0)
+ rng = cv2.applyColorMap((depth * 255).astype(np.uint8), cv2.COLORMAP_TURBO)
+ rng[~tf.valid] = (34, 32, 38)
+ self._panel(rng, "дальностный образ", "R1–R8", 0, out)
+
+ # 2. ON: ближе окружения — то, что торчит навстречу
+ on = _norm(lam.on, 0.0, 0.02)
+ self._panel(cv2.applyColorMap((on * 255).astype(np.uint8),
+ cv2.COLORMAP_INFERNO),
+ "ламина L1 · ON-контраст", "ближе фона", 1, out)
+
+ # 3. OFF: дальше окружения — провалы, ниши, окклюзионная тень.
+ # Шкала шире, чем у ON: провалы по дальности много глубже выступов.
+ off = _norm(lam.off, 0.0, 0.06)
+ off_img = cv2.applyColorMap((off * 255).astype(np.uint8), cv2.COLORMAP_BONE)
+ off_img[~tf.valid] = (34, 32, 38)
+ self._panel(off_img, "ламина L2 · OFF-контраст", "дальше фона", 2, out)
+
+ # 4. Кандидаты: лучи, попавшие в габарит, поверх приглушённой сцены.
+ # Цвет по новизне: знакомое тусклое, незнакомое горит.
+ base = np.where(tf.valid, 40 + depth * 95, 22).astype(np.uint8)
+ cand = cv2.cvtColor(base, cv2.COLOR_GRAY2BGR)
+ mark = np.zeros(cand.shape[:2], np.uint8)
+ hot_of = np.zeros(cand.shape[:2], np.float32)
+ for c in res.candidates:
+ rays = c.extra.get("rays") if isinstance(c.extra, dict) else None
+ if rays is None:
+ continue
+ ii, jj = rays
+ keep = (ii < cand.shape[0]) & (jj < cand.shape[1])
+ mark[ii[keep], jj[keep]] = 1
+ hot_of[ii[keep], jj[keep]] = float(np.clip(c.novelty, 0.0, 1.0))
+ if mark.any():
+ # утолщаем: одиночные лучи на уменьшенной панели не видно
+ k = np.ones((3, 5), np.uint8)
+ grown = cv2.dilate(mark, k)
+ hot_g = cv2.dilate(hot_of, k)
+ m = grown.astype(bool)
+ cand[m] = np.stack([np.full(m.sum(), 60),
+ (110 + 130 * hot_g[m]).astype(int),
+ (70 + 185 * hot_g[m]).astype(int)], axis=1)
+ self._panel(cand, "лобула LC11 · кандидаты",
+ f"{len(res.candidates)} шт.", 3, out)
+
+ s = self.s
+ cv2.line(out, (PAD * s, self.strip_h - 2 * s), (self.w - PAD * s, self.strip_h - 2 * s),
+ (52, 52, 62), s)
+ return out
+
+ # ------------------------------------------------------------------ кадр
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ cloud = self.cloud.render(res)
+ if cloud is None:
+ return None
+ out = np.empty((self.h, self.w, 3), np.uint8)
+ out[:self.strip_h] = self._strip(res)
+ out[self.strip_h:] = cloud
+ return out
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from .brain_view import BrainView
+ return BrainView.to_msg(img, stamp, frame_id)
diff --git a/ros2_ws/src/flyguard/flyguard/brain_view.py b/ros2_ws/src/flyguard/flyguard/brain_view.py
new file mode 100644
index 0000000..0f7ac1e
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/brain_view.py
@@ -0,0 +1,369 @@
+"""Схема мозга дрозофилы, подсвеченная живой активностью конвейера.
+
+Каждая стадия FlyGuard соответствует конкретному нейропилю, поэтому состояние
+системы показывается не графиками, а анатомической схемой: где сейчас «горит»,
+там и идёт работа. На демонстрации это замыкает цепочку из ТЗ —
+тоннель → облако точек → мозг мухи → «препятствие, 87 м».
+
+Вид дорсальный, схематичный, но с правильным взаимным расположением: по краям
+оптические доли (ламина → медулла → лобула и лобулярная пластинка), в центре
+грибовидные тела (чашечка → ножка → доли) и центральный комплекс
+(протоцеребральный мост, веерное тело, эллипсоидное тело, нодули), снизу —
+нисходящие нейроны с гигантским волокном.
+
+Отрисовка стоит 2–4 мс и вынесена под флаг `brain_view`, чтобы не попадать
+в замеры задержки основного тракта.
+"""
+from __future__ import annotations
+
+import array
+
+import numpy as np
+
+try:
+ import cv2
+except ImportError: # без OpenCV вид просто выключен
+ cv2 = None
+
+# Встроенные шрифты OpenCV (Hershey) знают только латиницу: любая кириллица
+# выходит рядом вопросительных знаков. Подписи здесь русские и составляют
+# половину смысла схемы, поэтому текст рисуется через PIL шрифтом DejaVu.
+try:
+ from PIL import Image, ImageDraw, ImageFont
+except ImportError:
+ Image = ImageDraw = ImageFont = None
+
+_FONT_PATHS = (
+ "/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
+ "/usr/share/fonts/truetype/dejavu/DejaVuSansCondensed.ttf",
+ "/usr/share/fonts/TTF/DejaVuSans.ttf",
+ "C:/Windows/Fonts/segoeui.ttf",
+ "C:/Windows/Fonts/arial.ttf",
+)
+_font_cache: dict[int, object] = {}
+
+
+def _font(px: int):
+ """Шрифт нужного кегля; None — если ни одного файла не нашлось."""
+ if ImageFont is None:
+ return None
+ if px not in _font_cache:
+ f = None
+ for path in _FONT_PATHS:
+ try:
+ f = ImageFont.truetype(path, px)
+ break
+ except OSError:
+ continue
+ _font_cache[px] = f or ImageFont.load_default()
+ return _font_cache[px]
+
+
+def _px(scale: float) -> int:
+ """Кегль PIL, дающий примерно ту же высоту, что и Hershey с этим масштабом."""
+ return max(9, int(round(scale * 30)))
+
+
+def text_size(txt: str, scale: float) -> tuple[int, int]:
+ f = _font(_px(scale))
+ if f is None:
+ (tw, th), _ = cv2.getTextSize(txt, cv2.FONT_HERSHEY_SIMPLEX, scale, 1)
+ return tw, th
+ box = f.getbbox(txt)
+ return box[2] - box[0], box[3] - box[1]
+
+
+def put_text(img, txt: str, org, scale: float, colour, weight: int = 1) -> None:
+ """Надпись с кириллицей. `org` — левый нижний угол, как у cv2.putText.
+
+ Рисуется по вырезанному прямоугольнику, а не по всему полотну: PIL копирует
+ изображение при каждом преобразовании, и на полном кадре полтора десятка
+ подписей стоили 90 мс — больше, чем весь конвейер.
+ """
+ f = _font(_px(scale))
+ if f is None or Image is None:
+ cv2.putText(img, txt, org, cv2.FONT_HERSHEY_SIMPLEX, scale, colour,
+ weight, cv2.LINE_AA)
+ return
+ x, y = int(org[0]), int(org[1])
+ box = f.getbbox(txt)
+ pad = 2
+ # Чернила лягут в x … x+(box[2]-box[0]) по горизонтали и
+ # y-(box[3]-box[1]) … y по вертикали — вырез берётся ровно по ним.
+ x0 = max(x - pad, 0)
+ y0 = max(y - (box[3] - box[1]) - pad, 0)
+ x1 = min(x + (box[2] - box[0]) + pad + 1, img.shape[1])
+ y1 = min(y + pad + 1, img.shape[0])
+ if x1 <= x0 or y1 <= y0:
+ return
+ crop = img[y0:y1, x0:x1]
+ pil = Image.fromarray(crop[:, :, ::-1])
+ ImageDraw.Draw(pil).text((x - box[0] - x0, y - box[3] - y0), txt, font=f,
+ fill=tuple(int(c) for c in colour[::-1]))
+ crop[:, :, :] = np.asarray(pil)[:, :, ::-1]
+
+W, H = 1180, 620
+BG = (22, 20, 26)
+INK = (150, 150, 165)
+
+# шкала «покой → работа → тревога» в BGR
+_STOPS = (
+ (0.00, (58, 48, 44)),
+ (0.30, (120, 96, 52)),
+ (0.55, (168, 178, 64)),
+ (0.78, (72, 196, 240)),
+ (1.00, (66, 66, 248)),
+)
+
+
+def _colour(a: float) -> tuple[int, int, int]:
+ a = float(np.clip(a, 0.0, 1.0))
+ for (x0, c0), (x1, c1) in zip(_STOPS, _STOPS[1:]):
+ if a <= x1:
+ t = (a - x0) / max(x1 - x0, 1e-6)
+ return tuple(int(c0[i] + (c1[i] - c0[i]) * t) for i in range(3))
+ return _STOPS[-1][1]
+
+
+def _annulus(cx, cy, r_out, r_in, a0, a1, squash=1.0):
+ """Кольцевой сектор — форма слоя оптической доли."""
+ # ellipse2Poly требует положительный шаг, поэтому внутренняя дуга строится
+ # в прямом направлении и разворачивается уже массивом
+ outer = cv2.ellipse2Poly((cx, cy), (r_out, int(r_out * squash)), 0, a0, a1, 4)
+ inner = cv2.ellipse2Poly((cx, cy), (r_in, int(r_in * squash)), 0, a0, a1, 4)
+ return np.vstack([outer, inner[::-1]])
+
+
+class Region:
+ __slots__ = ("key", "ru", "pts", "anchor", "label_at")
+
+ def __init__(self, key, ru, pts, label_at=None):
+ self.key = key
+ self.ru = ru
+ self.pts = np.asarray(pts, np.int32)
+ self.anchor = tuple(self.pts.mean(axis=0).astype(int))
+ self.label_at = label_at
+
+
+def _build_regions() -> list[Region]:
+ r: list[Region] = []
+ for side, (cx, a0, a1, lx) in enumerate(((250, 118, 242, 18), (W - 250, -62, 62, W - 250))):
+ cy = 300
+ mirror = side == 1
+ spec = [
+ ("lamina", "ламина L1 / L2", 218, 188),
+ ("medulla", "медулла T4 / T5", 182, 132),
+ ("lobula", "лобула LC11", 126, 88),
+ ("lobula_plate", "лоб. пластинка LPTC / LPLC2", 82, 52),
+ ]
+ for i, (key, ru, ro, ri) in enumerate(spec):
+ pts = _annulus(cx, cy, ro, ri, a0 + i * 3, a1 - i * 3, squash=1.06)
+ ly = 96 + i * 26
+ r.append(Region(key, ru, pts, (30 if not mirror else W - 330, ly)))
+
+ cx = W // 2
+
+ # грибовидные тела: чашечка сзади → ножка вперёд → вертикальная и медиальная
+ # доли. Они лежат ЛАТЕРАЛЬНЕЕ центрального комплекса, не поверх него.
+ for mirror in (False, True):
+ s = -1 if mirror else 1
+ bx = cx + s * 150
+ calyx = cv2.ellipse2Poly((bx, 132), (46, 33), 0, 0, 360, 6)
+ ped = np.array([[bx - s * 16, 160], [bx + s * 18, 158],
+ [bx + s * 46, 318], [bx + s * 18, 322]], np.int32)
+ lobes = np.array([[bx + s * 16, 316], [bx + s * 50, 314], [bx + s * 56, 404],
+ [bx + s * 26, 408], [bx + s * 26, 372], [bx + s * 16, 372]],
+ np.int32)
+ r.append(Region("mb_calyx", "чашечка PN → KC", calyx,
+ (cx - 330, 86) if not mirror else (cx + 210, 86)))
+ r.append(Region("mb_ped", "ножка · APL", ped, None))
+ r.append(Region("mb_lobes", "доли KC → MBON (новизна)", lobes,
+ (cx - 400, 432) if not mirror else (cx + 230, 432)))
+
+ pb = _annulus(cx, 232, 104, 86, 200, 340, squash=0.7)
+ fb = np.array([[cx - 74, 262], [cx + 74, 262], [cx + 58, 314], [cx - 58, 314]], np.int32)
+ eb = _annulus(cx, 372, 46, 26, 0, 360)
+ no_l = cv2.ellipse2Poly((cx - 52, 420), (18, 13), 0, 0, 360, 8)
+ no_r = cv2.ellipse2Poly((cx + 52, 420), (18, 13), 0, 0, 360, 8)
+ dn = np.array([[cx - 22, 444], [cx + 22, 444], [cx + 40, 566], [cx - 40, 566]], np.int32)
+
+ r += [
+ Region("pb", "протоцеребральный мост", pb, (cx - 96, 176)),
+ Region("fb", "веерное тело", fb, (cx - 40, 342)),
+ Region("eb", "эллипсоидное тело — кольцевой аттрактор", eb, (cx - 300, 372)),
+ Region("no", "нодули", no_l, None),
+ Region("no", "нодули", no_r, (cx + 96, 428)),
+ Region("dn", "нисходящие нейроны · гигантское волокно", dn, (cx - 300, 524)),
+ ]
+ return r
+
+
+class BrainView:
+ """Отрисовка схемы с активностью."""
+
+ def __init__(self, labels: bool = True):
+ self.enabled = cv2 is not None
+ self.labels = labels
+ self.regions = _build_regions() if self.enabled else []
+ self._base = self._draw_base() if self.enabled else None
+ self._smooth: dict[str, float] = {}
+
+ # ------------------------------------------------------------------ статика
+
+ def _draw_base(self) -> np.ndarray:
+ img = np.full((H, W, 3), BG, np.uint8)
+ put_text(img, "FlyGuard — схемы мозга Drosophila melanogaster в реальном времени",
+ (26, 36), 0.66, (205, 205, 215))
+ cv2.line(img, (26, 48), (W - 26, 48), (52, 52, 62), 1)
+ return img
+
+ # ------------------------------------------------------------------ активность
+
+ @staticmethod
+ def activity(res) -> dict[str, float]:
+ """Свести состояние кадра к активности по нейропилям, 0…1."""
+ lam = res.lam
+ d = res.decision
+ on = float(np.mean(lam.on)) * 130.0 if lam is not None else 0.0
+ off = float(np.mean(lam.off)) * 130.0 if lam is not None else 0.0
+ hole = float(np.mean(lam.hole)) if lam is not None else 0.0
+ speed = abs(res.ego.speed) / 20.0 if res.ego else 0.0
+ loom = (float(np.percentile(res.loom, 99.9)) * 25.0
+ if res.loom is not None and res.loom.size else 0.0)
+ nov = max((c.novelty for c in res.candidates), default=0.0)
+ # Дофамин: у мухи нейроны PPL1/PAM управляют пластичностью синапсов
+ # KC→MBON — ровно тем, чем учится наша память тоннеля. Осмысленная
+ # величина здесь — сила сигнала обучения: насколько сцена в целом
+ # незнакома памяти. В отличие от `nov` (самый незнакомый предмет)
+ # это средняя незнакомость всего, что видно, и она заранее говорит,
+ # что участок новый и ложных тревог будет больше.
+ dopa = (float(np.mean([c.novelty for c in res.candidates]))
+ if res.candidates else 0.0)
+ n_cand = min(len(res.candidates) / 7.0, 1.0)
+ evid = max((o.confidence for o in d.objects), default=0.0)
+
+ valid = float(lam.disp.astype(bool).mean()) if lam is not None else 0.0
+
+ return {
+ # Сетчатка «горит» тем сильнее, чем больше лучей вернулось: это и
+ # есть освещённость омматидиальной решётки.
+ "retina": np.clip(0.15 + 0.85 * valid, 0, 1),
+ "looming": np.clip(loom, 0, 1),
+ "dopamine": np.clip(dopa, 0, 1),
+ # Привыкание: доля синапсов KC→MBON, уже подавленных повторами
+ # внутри этого проезда. Ноль в начале участка, растёт по мере
+ # того, как однообразная обстановка перестаёт быть новостью.
+ "habituation": np.clip(getattr(res, "hab", 0.0), 0, 1),
+ "lamina": np.clip(0.55 * on + 0.45 * off, 0, 1),
+ "medulla": np.clip(0.25 + 0.75 * speed, 0, 1),
+ "lobula": np.clip(n_cand, 0, 1),
+ "lobula_plate": np.clip(max(speed, loom), 0, 1),
+ "mb_calyx": np.clip(n_cand, 0, 1),
+ "mb_ped": np.clip(0.4 * n_cand, 0, 1),
+ "mb_lobes": np.clip(nov, 0, 1),
+ "pb": np.clip(speed, 0, 1),
+ "fb": np.clip(hole, 0, 1),
+ "eb": np.clip(evid, 0, 1),
+ "no": np.clip(abs(res.ego.yaw_deg) / 1.5 if res.ego else 0.0, 0, 1),
+ "dn": 1.0 if d.emergency else (0.7 if d.detected else 0.06),
+ }
+
+ # ------------------------------------------------------------------ кадр
+
+ def render(self, res) -> np.ndarray | None:
+ if not self.enabled:
+ return None
+ for k, v in self.activity(res).items(): # сглаживание: мозг не мигает
+ self._smooth[k] = 0.6 * self._smooth.get(k, v) + 0.4 * v
+
+ img = self._base.copy()
+ for reg in self.regions:
+ a = self._smooth.get(reg.key, 0.0)
+ cv2.fillPoly(img, [reg.pts], _colour(a))
+ cv2.polylines(img, [reg.pts], True, (46, 44, 54), 1, cv2.LINE_AA)
+
+ self._draw_bump(img, res)
+ self._draw_flow(img)
+ if self.labels:
+ self._draw_labels(img)
+ self._draw_readout(img, res)
+ return img
+
+ def _draw_labels(self, img) -> None:
+ seen = set()
+ for reg in self.regions:
+ if reg.label_at is None or reg.ru in seen:
+ continue
+ seen.add(reg.ru)
+ x, y = reg.label_at
+ tw, th = text_size(reg.ru, 0.44)
+ # подложка под текстом: подписи неизбежно ложатся поверх нейропилей,
+ # и без неё их не прочитать
+ box = img[max(y - th - 6, 0):y + 6, max(x - 6, 0):x + tw + 6]
+ if box.size:
+ box[:] = (box * 0.25).astype(np.uint8)
+ cv2.line(img, (x + tw // 2, y - th - 6 if y > reg.anchor[1] else y + 6),
+ reg.anchor, (74, 72, 86), 1, cv2.LINE_AA)
+ put_text(img, reg.ru, (x, y), 0.44, INK)
+
+ def _draw_bump(self, img, res) -> None:
+ """Бугор активности в эллипсоидном теле — направление на ближайший объект."""
+ cx, cy, r = W // 2, 380, 36
+ objs = res.decision.objects
+ if not objs:
+ return
+ o = objs[0]
+ ang = float(np.clip(np.arctan2(o.lateral, max(o.distance, 1.0)) * 7.0, -3.0, 3.0))
+ px = int(cx + r * np.sin(ang))
+ py = int(cy - r * np.cos(ang))
+ cv2.circle(img, (px, py), 11, _colour(min(o.confidence + 0.35, 1.0)), -1, cv2.LINE_AA)
+ cv2.circle(img, (px, py), 11, (230, 230, 240), 1, cv2.LINE_AA)
+
+ def _draw_flow(self, img) -> None:
+ cx = W // 2
+ for a, b in (((330, 300), (410, 260)), ((W - 330, 300), (W - 410, 260)),
+ ((cx - 60, 336), (cx - 30, 356)), ((cx + 60, 336), (cx + 30, 356)),
+ ((cx, 420), (cx, 428))):
+ cv2.arrowedLine(img, a, b, (86, 84, 98), 1, cv2.LINE_AA, tipLength=0.3)
+
+ def _draw_readout(self, img, res) -> None:
+ d = res.decision
+ rows = [
+ ("скорость", f"{res.ego.kmh:.1f} км/ч" if res.ego else "—"),
+ ("кандидатов", str(len(res.candidates))),
+ ("новизна MBON", f"{max((c.novelty for c in res.candidates), default=0.0):.2f}"),
+ ("треков", str(len(d.objects))),
+ ("обработка", f"{res.total_ms:.1f} мс"),
+ ]
+ y0 = H - 150
+ for i, (k, v) in enumerate(rows):
+ put_text(img, k, (28, y0 + i * 24), 0.46, (118, 116, 128))
+ put_text(img, v, (170, y0 + i * 24), 0.5, (206, 206, 216))
+
+ if d.detected:
+ txt = f"ПРЕПЯТСТВИЕ {d.distance:.0f} м"
+ sub = ("ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency
+ else f"уверенность {d.confidence:.2f}")
+ colour = (66, 66, 248) if d.emergency else (72, 196, 240)
+ else:
+ txt, sub, colour = "ПУТЬ СВОБОДЕН", "", (120, 214, 130)
+ put_text(img, txt, (W - 470, H - 90), 0.95, colour, 2)
+ if sub:
+ put_text(img, sub, (W - 470, H - 58), 0.56, colour)
+
+ # ------------------------------------------------------------------ ROS
+
+ @staticmethod
+ def to_msg(img: np.ndarray, stamp, frame_id: str):
+ from sensor_msgs.msg import Image
+ msg = Image()
+ msg.header.stamp = stamp
+ msg.header.frame_id = frame_id
+ msg.height, msg.width = img.shape[:2]
+ msg.encoding = "bgr8"
+ msg.is_bigendian = 0
+ msg.step = img.shape[1] * 3
+ # array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python,
+ # и картинка в 2.8 МБ стоила 142 мс на кадр против 2.4 мс (замерено).
+ msg.data = array.array("B", img.tobytes())
+ return msg
diff --git a/ros2_ws/src/flyguard/flyguard/cdr.py b/ros2_ws/src/flyguard/flyguard/cdr.py
new file mode 100644
index 0000000..0d139da
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/cdr.py
@@ -0,0 +1,149 @@
+"""Разбор sensor_msgs/msg/PointCloud2 из CDR без зависимости от ROS.
+
+Нужен для двух сценариев:
+ * офлайн-эксперименты на машине без ROS (Windows);
+ * прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`.
+
+Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же
+`PointCloud2` функция `from_ros_message`.
+"""
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+
+import numpy as np
+
+# sensor_msgs/msg/PointField: код типа -> (numpy dtype, размер в байтах)
+_PF_DTYPES = {
+ 1: ("i1", 1), 2: ("u1", 1), 3: ("i2", 2), 4: ("u2", 2),
+ 5: ("i4", 4), 6: ("u4", 4), 7: ("f4", 4), 8: ("f8", 8),
+}
+
+
+@dataclass(frozen=True)
+class PointCloud2:
+ """Минимальное представление облака точек."""
+
+ stamp: float
+ frame_id: str
+ height: int
+ width: int
+ point_step: int
+ is_dense: bool
+ points: np.ndarray # структурированный массив длиной height*width
+
+ @property
+ def n_points(self) -> int:
+ return int(self.points.shape[0])
+
+
+class _CdrReader:
+ """Чтение little-endian CDR с выравниванием примитивов относительно тела сообщения."""
+
+ __slots__ = ("buf", "origin", "pos")
+
+ def __init__(self, buf: bytes | memoryview):
+ self.buf = buf
+ self.origin = 4 # заголовок инкапсуляции
+ self.pos = 4
+
+ def _align(self, size: int) -> None:
+ self.pos += (-(self.pos - self.origin)) % size
+
+ def u8(self) -> int:
+ v = self.buf[self.pos]
+ self.pos += 1
+ return v
+
+ def u32(self) -> int:
+ self._align(4)
+ v = struct.unpack_from(" int:
+ self._align(4)
+ v = struct.unpack_from(" str:
+ n = self.u32()
+ s = bytes(self.buf[self.pos:self.pos + max(n - 1, 0)]).decode("utf-8", "replace")
+ self.pos += n
+ return s
+
+
+def point_dtype(fields: list[tuple[str, int, int, int]], point_step: int) -> np.dtype:
+ """Собрать numpy-dtype по описанию полей, явно добивая пропуски паддингом.
+
+ Поля лидара невыровнены (`timestamp` float64 по смещению 18), поэтому
+ структурированный dtype строится вручную, а не через `np.dtype(align=True)`.
+ """
+ spec: list[tuple[str, str]] = []
+ used = 0
+ for name, offset, datatype, count in fields:
+ kind, size = _PF_DTYPES[datatype]
+ if offset > used:
+ spec.append((f"_pad{used}", f"V{offset - used}"))
+ elif offset < used:
+ raise ValueError(f"перекрывающиеся поля в PointCloud2: {name}")
+ spec.append((name, kind if count == 1 else f"{count}{kind}"))
+ used = offset + size * count
+ if point_step > used:
+ spec.append((f"_pad{used}", f"V{point_step - used}"))
+ dt = np.dtype(spec)
+ if dt.itemsize != point_step:
+ raise ValueError(f"dtype {dt.itemsize} байт != point_step {point_step}")
+ return dt
+
+
+def parse_pointcloud2(blob: bytes | memoryview) -> PointCloud2:
+ """Разобрать CDR-сериализованное sensor_msgs/msg/PointCloud2."""
+ r = _CdrReader(blob)
+ sec = r.i32()
+ nsec = r.u32()
+ frame_id = r.string()
+ height = r.u32()
+ width = r.u32()
+
+ fields = []
+ for _ in range(r.u32()):
+ name = r.string()
+ offset = r.u32()
+ datatype = r.u8()
+ count = r.u32()
+ fields.append((name, offset, datatype, count))
+
+ r.u8() # is_bigendian: в данных всегда 0, little-endian
+ point_step = r.u32()
+ r.u32() # row_step
+ n_bytes = r.u32()
+ data = memoryview(blob)[r.pos:r.pos + n_bytes]
+ r.pos += n_bytes
+ is_dense = bool(r.u8())
+
+ dt = point_dtype(fields, point_step)
+ points = np.frombuffer(data, dtype=dt, count=height * width)
+ 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)
diff --git a/ros2_ws/src/flyguard/flyguard/central_complex.py b/ros2_ws/src/flyguard/flyguard/central_complex.py
new file mode 100644
index 0000000..d1b3df0
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/central_complex.py
@@ -0,0 +1,418 @@
+"""CENTRAL COMPLEX — накопление улик и треки в мировой системе координат.
+
+Эллипсоидное тело мухи держит **кольцевой аттрактор**: клетки EPG образуют один
+«бугор» активности, кодирующий текущий курс, клетки PEN сдвигают этот бугор по
+сигналам собственного вращения, а взаимное торможение не даёт возникнуть второму
+бугру. Так муха помнит направление, даже когда ориентир пропал из виду.
+
+Здесь тот же механизм решает другую задачу. Кандидат на дальности 150 м — это
+пять-десять лучей, и по одному кадру он неотличим от шума. Но поезд едет, и
+объект, если он настоящий, остаётся **на одном и том же месте в тоннеле**, а не в
+поле зрения. Поэтому треки живут в координате «расстояние по пути от точки
+старта», сдвигаемой оценкой собственного движения (роль PEN), каждое совпадение
+подкачивает улику (локальное возбуждение), несовпадение — утечка, а конкуренция
+за одно и то же место не даёт плодить дубликаты (глобальное торможение APL/Δ7).
+
+Это и есть подтверждение по нескольким кадрам, которого требует ТЗ, — но не как
+эвристический фильтр, а как накопитель, который вытягивает слабый сигнал из шума.
+"""
+from __future__ import annotations
+
+import itertools
+from dataclasses import dataclass, field
+
+import numpy as np
+
+from .lobula import Candidate
+
+
+@dataclass
+class Track:
+ """Подтверждаемая гипотеза о препятствии."""
+
+ id: int
+ s_world: float # положение вдоль пути от начала записи, м
+ u: float # смещение от оси пути, м
+ h: float # высота над рельсом, м
+ width: float
+ height: float
+ evidence: float = 0.0 # накопленная улика, 0…1
+ hits: int = 0
+ misses: int = 0
+ age: int = 0
+ first_d: float = 0.0 # на какой дальности впервые замечен
+ last_d: float = 0.0
+ last_n_rays: int = 0
+ novelty: float = 1.0
+ speed_lat: float = 0.0 # поперечная скорость, м/с
+ p_track: float = 0.0 # отсчёт считывания по треку, если оно подключено
+ # То же смещение в системе лидара, без поправки на кривую. В решении не
+ # участвует: по нему рамка рисуется у своих точек, а не у оси пути.
+ u_raw: float = 0.0
+ history: list = field(default_factory=list)
+
+ # Сводка по всем наблюдениям трека, накапливаемая на лету. Считать её из
+ # `history` нельзя: описание трека нужно каждый кадр для всех живых
+ # треков, а разворачивать ради этого полсотни списков в массивы — заметная
+ # доля бюджета кадра. Все обновления здесь O(1).
+ n_obs: int = 0
+ s_sum: float = 0.0 # мировая привязка, подразумеваемая наблюдением
+ s_sq: float = 0.0
+ u_sum: float = 0.0
+ u_sq: float = 0.0
+ w_sum: float = 0.0 # вес наблюдения — ровно то, что идёт в улику
+ w_max: float = 0.0
+ p_sum: float = 0.0 # отсчёт считывания по кандидату, если оно есть
+ p_max: float = 0.0
+ p_n: int = 0 # сколько раз он вообще был
+ w_ema: float = 0.0 # он же, но со скользящим забыванием
+ r_sum: float = 0.0 # лучи, нормированные на ожидаемые для дальности
+ r_last: float = 0.0
+ ri_sum: float = 0.0 # суммы для наклона нормированных лучей по времени
+ i_sum: float = 0.0
+ i_sq: float = 0.0
+
+ @property
+ def confirmed(self) -> bool:
+ return self.evidence >= 0.5
+
+ @property
+ def w_mean(self) -> float:
+ """Средний вес наблюдения за жизнь трека.
+
+ Улика — это тот же вес, но просуммированный и обрезанный единицей, и
+ обрезание её убивает: на реальном объекте медиана улики 1.000 и у
+ предмета, и у ложных треков (AUC 0.624), а у среднего веса 0.998
+ против 0.269 (AUC 1.000). Сумма отвечает «давно ли я на это смотрю»,
+ среднее — «на что именно я смотрю». EXPERIMENTS п. 15.
+ """
+ return self.w_sum / max(self.n_obs, 1)
+
+ @property
+ def p_mean(self) -> float:
+ """Средний покадровый отсчёт считывания MBON за жизнь трека.
+
+ Без переданной конвейеру модели отсчёта нет вовсе, и возвращать тут
+ ноль значило бы тихо погасить все треки разом. Поэтому в таком случае
+ отдаётся средний вес наблюдения — он считается всегда.
+ """
+ return self.p_sum / self.p_n if self.p_n else self.w_mean
+
+ def distance(self, s_now: float) -> float:
+ return self.s_world - s_now
+
+ def observe(self, c: Candidate, s_obs: float, w: float) -> None:
+ """Записать наблюдение в сводку.
+
+ Полезное здесь — `w_sum` и `p_sum`: улика суммируется и упирается в
+ потолок, а среднее того же веса различает предмет и обстановку и после
+ насыщения (EXPERIMENTS п. 15.1).
+
+ `s_obs` — место в тоннеле, которое подразумевает наблюдение: путь
+ плюс дальность. Копится ТОЛЬКО ради замеров: на полигоне его разброс
+ выглядит сильным признаком, а на настоящем объекте он перевёрнут,
+ потому что вставка стоит на дальности, посчитанной из той же оценки
+ движения (п. 15.4). В решении не участвует и участвовать не должен.
+ """
+ i = float(self.n_obs)
+ self.n_obs += 1
+ self.s_sum += s_obs
+ self.s_sq += s_obs * s_obs
+ self.u_sum += c.u
+ self.u_sq += c.u * c.u
+ self.w_sum += w
+ self.w_max = max(self.w_max, w)
+ # Среднее за всю жизнь отстаёт у СБЛИЖАЮЩЕГОСЯ предмета: он
+ # появляется далеко и слабым, и первые десятки наблюдений тянут
+ # среднее вниз ещё долго после того, как он стал отчётливым.
+ # Скользящее забывание (полупериод около трёх наблюдений) отвечает
+ # на вопрос «каков он сейчас», а не «каким был с самого начала».
+ self.w_ema = w if self.n_obs == 1 else 0.8 * self.w_ema + 0.2 * w
+ p = c.extra.get("mbon") if c.extra else None
+ if p is not None:
+ self.p_n += 1
+ self.p_sum += float(p)
+ self.p_max = max(self.p_max, float(p))
+ r = c.n_rays / expected_rays(c.d)
+ self.r_sum += r
+ self.r_last = r
+ self.ri_sum += i * r
+ self.i_sum += i
+ self.i_sq += i * i
+
+
+class CentralComplex:
+ """Накопитель улик и менеджер треков."""
+
+ def __init__(self, *, gate_d: float = 4.0, gate_u: float = 1.2,
+ gain: float = 0.34, leak: float = 0.12,
+ leak_far: float | None = None, leak_far_from: float = 90.0,
+ inhibition: float = 0.05, max_misses: int = 12,
+ max_tracks: int = 48, use_shape: bool = True,
+ mbon_power: float = 1.0, mbon_blend: float = 1.0,
+ nov_fade_from: float = 0.0, nov_fade_to: float = 160.0,
+ lying_exempt: bool = False, top_from: float = float("inf"),
+ flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)):
+ self.use_shape = use_shape
+ self.lying_exempt = lying_exempt
+ self.top_from = top_from # низ выше этого — предмет в верхней секции габарита
+ self.flat = flat # (высота, верх, дальность, множитель), см. _quality
+ self.mbon_power = mbon_power
+ self.mbon_blend = mbon_blend
+ self.nov_fade_from = nov_fade_from
+ self.nov_fade_to = nov_fade_to
+ self.gate_d = gate_d
+ self.gate_u = gate_u
+ self.gain = gain
+ self.leak = leak
+ # Утечка улики за промах, слабеющая с дальностью. Промах на 150 м
+ # — не свидетельство отсутствия, а свойство решётки: предмет там
+ # даёт единицы лучей и виден через кадр. Ровно по этой причине
+ # число лучей уже нормируется на ожидаемое (см. _quality), и
+ # промах нормировать надо так же.
+ #
+ # Считать легко: при доле попаданий p улика не убывает, только
+ # если p·gain·w > (1-p)·leak. На 120…160 м кандидат появляется в
+ # 62 % наблюдений, и при gain 0.34 и leak 0.12 нужно качество
+ # выше 0.22 — столько далёкое наблюдение не даёт никогда. Поэтому
+ # улика у далёкого трека не «чуть ниже порога», а около нуля, и
+ # снижением порога это не лечится (замер: 0.5 → 0.3 не изменил
+ # ничего). None — зависимости нет, как было.
+ self.leak_far = leak if leak_far is None else leak_far
+ self.leak_far_from = leak_far_from
+ self.inhibition = inhibition
+ self.max_misses = max_misses
+ self.max_tracks = max_tracks
+ self.tracks: list[Track] = []
+ self.s_world = 0.0
+ self._ids = itertools.count(1)
+
+ # ------------------------------------------------------------------ обновление
+
+ def _leak_at(self, d: float) -> float:
+ """Утечка на дальности d: линейно от leak до leak_far к 200 м."""
+ span = 200.0 - self.leak_far_from
+ if span <= 0 or d <= self.leak_far_from:
+ return self.leak
+ f = min((d - self.leak_far_from) / span, 1.0)
+ return self.leak + (self.leak_far - self.leak) * f
+
+ def update(self, candidates: list[Candidate], ds: float, dt: float) -> list[Track]:
+ """Сдвинуть мир на `ds`, сопоставить кандидатов, обновить улики."""
+ self.s_world += ds
+
+ # допуск по дальности растёт с расстоянием: там и разрешение грубее,
+ # и ошибка оценки собственного движения успевает накопиться
+ for t in self.tracks:
+ t.age += 1
+
+ used = set()
+ for t in self.tracks:
+ d_pred = t.distance(self.s_world)
+ best, best_cost = None, None
+ for k, c in enumerate(candidates):
+ if k in used:
+ continue
+ gd = self.gate_d + 0.05 * max(c.d, 0.0)
+ dd = abs(c.d - d_pred)
+ du = abs(c.u - t.u)
+ if dd > gd or du > self.gate_u + 0.4:
+ continue
+ cost = dd / gd + du / (self.gate_u + 0.4)
+ if best_cost is None or cost < best_cost:
+ best, best_cost = k, cost
+ if best is None:
+ t.misses += 1
+ t.evidence = max(0.0, t.evidence
+ - self._leak_at(t.distance(self.s_world)))
+ continue
+
+ c = candidates[best]
+ used.add(best)
+ t.hits += 1
+ t.misses = 0
+ w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
+ self.nov_fade_from, self.nov_fade_to,
+ self.lying_exempt, self.top_from, self.flat)
+ t.observe(c, self.s_world + c.d, w)
+ t.evidence = min(1.0, t.evidence + self.gain * w)
+ if dt > 1e-3:
+ t.speed_lat = 0.6 * t.speed_lat + 0.4 * (c.u - t.u) / dt
+ # положение сглаживается: ближние наблюдения точнее дальних
+ a = float(np.clip(0.5 * (60.0 / max(c.d, 20.0)), 0.15, 0.6))
+ t.s_world = (1 - a) * t.s_world + a * (self.s_world + c.d)
+ t.u = (1 - a) * t.u + a * c.u
+ t.u_raw = (1 - a) * t.u_raw + a * float(c.extra.get("u_raw", c.u))
+ t.h = (1 - a) * t.h + a * c.h
+ t.width = max(t.width * 0.7, c.width)
+ t.height = max(t.height * 0.7, c.height)
+ t.last_d = c.d
+ t.last_n_rays = c.n_rays
+ t.novelty = 0.7 * t.novelty + 0.3 * c.novelty
+ t.history.append((c.d, c.u, c.n_rays))
+ if len(t.history) > 64:
+ del t.history[:-64]
+
+ # новые гипотезы из несопоставленных кандидатов
+ for k, c in enumerate(candidates):
+ if k in used or len(self.tracks) >= self.max_tracks:
+ continue
+ t = Track(id=next(self._ids), s_world=self.s_world + c.d, u=c.u, h=c.h,
+ width=c.width, height=c.height, first_d=c.d, last_d=c.d,
+ last_n_rays=c.n_rays, novelty=c.novelty,
+ u_raw=float(c.extra.get("u_raw", c.u)))
+ w = _quality(c, self.use_shape, self.mbon_power, self.mbon_blend,
+ self.nov_fade_from, self.nov_fade_to,
+ self.lying_exempt, self.top_from, self.flat)
+ t.observe(c, self.s_world + c.d, w)
+ t.evidence = self.gain * w
+ t.hits = 1
+ self.tracks.append(t)
+
+ # глобальное торможение: сильный трек подавляет соседей по месту
+ self._inhibit()
+
+ self.tracks = [t for t in self.tracks
+ if t.misses <= self.max_misses and t.evidence > 0.02
+ and t.distance(self.s_world) > -5.0]
+ self.tracks.sort(key=lambda t: -t.evidence)
+ del self.tracks[self.max_tracks:]
+ return self.tracks
+
+ def _inhibit(self) -> None:
+ if len(self.tracks) < 2 or self.inhibition <= 0:
+ return
+ order = sorted(self.tracks, key=lambda t: -t.evidence)
+ for i, strong in enumerate(order):
+ for weak in order[i + 1:]:
+ if (abs(strong.s_world - weak.s_world) < self.gate_d
+ and abs(strong.u - weak.u) < self.gate_u):
+ weak.evidence = max(0.0, weak.evidence - self.inhibition)
+
+ def confirmed(self) -> list[Track]:
+ out = [t for t in self.tracks if t.confirmed and t.distance(self.s_world) > 0]
+ out.sort(key=lambda t: t.distance(self.s_world))
+ return out
+
+
+def expected_rays(d: float) -> float:
+ """Сколько лучей предмет даёт на дальности d просто по устройству решётки.
+
+ Нужна в двух местах — в весе наблюдения и в описании трека, — и оба
+ раза за одним и тем же: дальний предмет даёт мало лучей не потому, что
+ он сомнительный, а потому, что так устроена развёртка.
+ """
+ return max(3.0, 2500.0 / max(d, 5.0) ** 1.4)
+
+
+def _quality(c: Candidate, use_shape: bool = True, mbon_power: float = 1.0,
+ mbon_blend: float = 1.0, nov_fade_from: float = 0.0,
+ nov_fade_to: float = 160.0, lying_exempt: bool = False,
+ top_from: float = float("inf"),
+ flat: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)) -> float:
+ """Вес одного наблюдения: сколько улики оно добавляет.
+
+ Дальний объект даёт мало лучей не потому, что он сомнительный, а потому что
+ так устроена решётка, — поэтому число лучей нормируется на ожидаемое для
+ этой дальности. Остальные множители отделяют предмет от конструкции тоннеля:
+ предмет целиком помещается в габарит и компактен вдоль пути, а лоток или
+ стена тянутся дальше и в стороны. Новизна из грибовидного тела входит сюда
+ же множителем.
+ """
+ # Висящее в верхней секции (`top_from`) — отдельная статья. Обе обученные
+ # части, считывание MBON и память тоннеля, собирались до того, как у
+ # габарита появилась верхняя секция, и таких форм не видели: их ответ там
+ # не значит ничего, и оба канала здесь выключены. Нормировка числа лучей
+ # рассчитана на предмет с человека, а свисающий кабель или стержень тонок
+ # по природе: у стержня 5 см опора упиралась в нижний край 0.25 на любой
+ # дальности. Пояс над вагоном в исправном тоннеле пуст, поэтому нижний
+ # край для висящего поднят до 0.5. EXPERIMENTS п. 19.
+ top = c.h_min >= top_from
+ support = float(np.clip(c.n_rays / expected_rays(c.d), 0.5 if top else 0.25, 1.0))
+
+ # Контраст к фону — главная улика вблизи и недоступная вдали. На 170 м
+ # кольцо окружения ламины упирается в стену тоннеля, стоящую на той же
+ # дальности, и `gap` структурно равен нулю: предмет не «ближе окружения»,
+ # он «на той же дальности, что окружение». Замер это подтверждает —
+ # у вставленного человека на 140…185 м gap = 0.0 во ВСЕХ кадрах.
+ #
+ # Там, где контраст не измеряется, его заменяет опора веерного тела:
+ # возвращались ли лучи из этой точки пути кадр за кадром. Берётся
+ # максимум — вблизи решает контраст, вдали накопление.
+ contrast = float(np.clip(c.gap / 3.0, 0.2, 1.0))
+ acc = float(c.extra.get("acc_support", 0.0)) if c.extra else 0.0
+ if acc > 0.0:
+ contrast = max(contrast, float(np.clip(acc, 0.2, 1.0)))
+ if use_shape:
+ whole = float(np.clip((c.containment - 0.25) / 0.45, 0.05, 1.0))
+ span = max(c.width, c.height, 0.2)
+ # Предмет или человек, лежащий в колее вдоль пути (depth ~ 1.8 м при малой высоте),
+ # лежит прямо на полотне перед поездом — не штрафуем его как конструкцию тоннеля:
+ is_lying_on_track = (lying_exempt and abs(c.u) <= 0.85
+ and c.h <= 0.40 and c.depth <= 2.2)
+ if is_lying_on_track:
+ compact = 1.0
+ else:
+ compact = float(np.clip(1.5 - c.depth / (3.0 * span), 0.1, 1.0))
+ # опора снизу: упавший предмет, человек, камень стоят на полотне, а знак,
+ # лоток или кронштейн висят на стене, и под ними пусто. Кроме верхней
+ # секции габарита (`top_from`): у оси под сводом стен нет, и висящее
+ # там — это то, что свисает в путь вагона.
+ grounded = 1.0 if top else float(np.clip(1.25 - c.h_min / 1.2, 0.15, 1.0))
+ else:
+ whole = compact = grounded = 1.0
+ # Знакомость на большой дальности не просто бесполезна, а вредна. Замер:
+ # на 120…185 м у вставленного человека новизна 0.150, а у окружающей
+ # обстановки 0.199, разделяющая способность AUC 0.293 — то есть
+ # признак перевёрнут. Причина понятна: на шести лучах дескриптор
+ # вырождается, и память узнаёт в предмете любую далёкую конструкцию.
+ # Поэтому вклад новизны гасится с дальностью, а вблизи, где она и
+ # работает, остаётся как был. 0 — не гасить (EXPERIMENTS п. 12.3).
+ novel = 1.0 if top else novelty_gain(c.novelty)
+ if not top and nov_fade_from > 0.0 and c.d > nov_fade_from:
+ span = max(nov_fade_to - nov_fade_from, 1e-3)
+ w = 1.0 - min((c.d - nov_fade_from) / span, 1.0)
+ novel = novel ** w
+ hand = support * contrast * whole * compact * grounded
+
+ # Обученное считывание MBON, если оно есть. Модель видит те же признаки,
+ # что и шесть множителей выше, плюс тень и интенсивность, которых в ручной
+ # формуле нет вовсе. Знакомость в неё НЕ входит и остаётся отдельным
+ # каналом: физику решает модель, конкретный тоннель — память.
+ #
+ # `mbon_blend` — геометрическое смешивание с ручной формулой: 1 — только
+ # модель, 0 — только руками, между ними всё промежуточное. Нужно затем,
+ # чтобы размен «модель против ручной формулы» мерился, а не объявлялся.
+ p = c.extra.get("mbon") if c.extra else None
+ # висящее в верхней секции модель не видела ни разу (обучена на стоящих
+ # предметах) и гасит его — здесь решает ручная формула
+ if p is not None and mbon_blend > 0.0 and not top:
+ pm = float(np.clip(p, 1e-4, 1.0)) ** max(mbon_power, 1e-3)
+ b = float(np.clip(mbon_blend, 0.0, 1.0))
+ hand = pm ** b * max(hand, 1e-4) ** (1.0 - b)
+ # Плоское у самого пола: высотой в сантиметры, целиком ниже `flat[1]` над
+ # рельсом. Это пластины и кромки на полотне, края жёлоба, порог
+ # гермозатвора — по ответу организаторов то, что в жёлобе, препятствием не
+ # считается. Модель таким ставит 0.8–0.99: в её обучении был кабель на
+ # путях. А ложных тревог это давало больше всего: порог гермозатвора на
+ # 65→31 м и два фантома на синтетике. Настоящий низкий предмет (0.3 м на
+ # рельсе, 2×0.2 м поперёк путей) вблизи даёт 0.1–0.4 м по высоте и верх
+ # выше 0.4 м. Дальше `flat[2]` решётка слишком редкая, чтобы судить о
+ # высоте, и там правило молчит. Множитель, а не запрет: если предмет хоть в
+ # одном кадре покажет высоту, он своё доберёт. EXPERIMENTS п. 19.
+ fh, ftop, fd, fw = flat
+ if fh > 0.0 and c.d <= fd and c.height < fh and c.h_min + c.height < ftop:
+ hand *= fw
+ return float(np.clip(hand * novel, 0.0, 1.0))
+
+
+def novelty_gain(novelty: float, lo: float = 0.15, hi: float = 0.50,
+ floor: float = 0.05) -> float:
+ """Ответ MBON → множитель улики.
+
+ Пороги взяты по измеренному разделению (`tools/tune_memory.py`): знакомая
+ обстановка даёт новизну около 0.14, реальный объект — около 0.50. Отображение
+ делает разницу резкой, но оставляет ненулевой пол: даже похожий на привычную
+ конструкцию предмет должен накапливать улику, просто медленнее.
+ """
+ return float(np.clip((novelty - lo) / (hi - lo), floor, 1.0))
diff --git a/ros2_ws/src/flyguard/flyguard/data/brain_atlas.npz b/ros2_ws/src/flyguard/flyguard/data/brain_atlas.npz
new file mode 100644
index 0000000..03010ba
Binary files /dev/null and b/ros2_ws/src/flyguard/flyguard/data/brain_atlas.npz differ
diff --git a/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz b/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz
new file mode 100644
index 0000000..deae590
Binary files /dev/null and b/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x2.npz differ
diff --git a/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x3.npz b/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x3.npz
new file mode 100644
index 0000000..7c62e17
Binary files /dev/null and b/ros2_ws/src/flyguard/flyguard/data/brain_atlas_x3.npz differ
diff --git a/ros2_ws/src/flyguard/flyguard/data/pandar128_channels.csv b/ros2_ws/src/flyguard/flyguard/data/pandar128_channels.csv
new file mode 100644
index 0000000..85be719
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/data/pandar128_channels.csv
@@ -0,0 +1,129 @@
+channel,az_offset_deg,elevation_deg,range_min_m,range_max_m,near_field,max_range_10pct_m,far_field,min_reflectivity,high_res
+1,3.257,14.436,0.3,100.0,1,100.0,0,100 m @ 10%,0
+2,3.263,13.535,2.7,100.0,0,100.0,0,100 m @ 10%,0
+3,1.091,13.082,0.3,100.0,1,100.0,0,100 m @ 10%,0
+4,3.268,12.624,2.7,100.0,0,100.0,0,100 m @ 10%,0
+5,1.093,12.165,2.7,100.0,0,100.0,0,100 m @ 10%,0
+6,3.273,11.702,0.3,100.0,1,120.0,0,100 m @ 6%,0
+7,1.094,11.239,2.7,100.0,0,120.0,0,100 m @ 6%,0
+8,3.278,10.771,2.7,100.0,0,140.0,0,100 m @ 3%,0
+9,1.095,10.305,0.3,100.0,1,140.0,0,100 m @ 3%,0
+10,3.283,9.83,2.7,100.0,0,140.0,0,100 m @ 3%,0
+11,1.096,9.356,2.7,100.0,0,140.0,0,100 m @ 3%,0
+12,3.288,8.88,0.3,100.0,1,140.0,0,100 m @ 3%,0
+13,1.097,8.401,2.7,100.0,0,140.0,0,100 m @ 3%,0
+14,3.291,7.921,2.7,100.0,0,140.0,0,100 m @ 3%,0
+15,1.098,7.438,0.3,100.0,1,140.0,0,100 m @ 3%,0
+16,-1.101,6.953,2.7,100.0,0,140.0,0,100 m @ 3%,0
+17,1.1,6.467,2.7,100.0,0,140.0,0,100 m @ 3%,0
+18,-1.104,5.978,0.3,100.0,1,140.0,0,100 m @ 3%,0
+19,-3.306,5.487,2.7,100.0,0,140.0,0,100 m @ 3%,0
+20,-1.106,4.996,2.7,100.0,0,140.0,0,100 m @ 3%,0
+21,-3.311,4.501,0.3,100.0,1,140.0,0,100 m @ 3%,0
+22,-1.109,4.007,2.7,100.0,0,140.0,0,100 m @ 3%,0
+23,-3.318,3.509,2.7,100.0,0,140.0,0,100 m @ 3%,0
+24,-1.111,3.013,0.3,100.0,1,140.0,0,100 m @ 3%,0
+25,-3.324,2.512,2.7,100.0,0,140.0,0,100 m @ 3%,0
+26,-1.113,2.013,0.3,200.0,1,140.0,0,200 m @ 37%,1
+27,7.72,1.885,2.7,200.0,0,140.0,0,200 m @ 37%,1
+28,5.535,1.761,2.7,200.0,0,140.0,0,200 m @ 37%,1
+29,3.325,1.637,2.7,200.0,0,140.0,0,200 m @ 37%,1
+30,-3.33,1.511,2.7,200.0,0,140.0,0,200 m @ 37%,1
+31,1.107,1.386,2.7,200.0,0,140.0,0,200 m @ 37%,1
+32,-5.538,1.258,2.7,200.0,0,140.0,0,200 m @ 37%,1
+33,-7.726,1.13,0.3,200.0,1,140.0,0,200 m @ 37%,1
+34,-1.115,1.008,2.7,200.0,0,200.0,1,200 m @ 10%,1
+35,7.731,0.88,2.7,200.0,0,200.0,1,200 m @ 10%,1
+36,5.543,0.756,2.7,200.0,0,200.0,1,200 m @ 10%,1
+37,3.329,0.63,2.7,200.0,0,200.0,1,200 m @ 10%,1
+38,-3.336,0.505,2.7,200.0,0,200.0,1,200 m @ 10%,1
+39,1.108,0.379,2.7,200.0,0,200.0,1,200 m @ 10%,1
+40,-5.547,0.251,0.3,200.0,1,200.0,1,200 m @ 10%,1
+41,-7.738,0.124,2.7,200.0,0,200.0,1,200 m @ 10%,1
+42,-1.117,0.0,2.7,200.0,0,200.0,1,200 m @ 10%,1
+43,7.743,-0.129,2.7,200.0,0,200.0,1,200 m @ 10%,1
+44,5.551,-0.254,2.7,200.0,0,200.0,1,200 m @ 10%,1
+45,3.335,-0.38,2.7,200.0,0,200.0,1,200 m @ 10%,1
+46,-3.342,-0.506,2.7,200.0,0,200.0,1,200 m @ 10%,1
+47,1.11,-0.632,0.3,200.0,1,200.0,1,200 m @ 10%,1
+48,-5.555,-0.76,2.7,200.0,0,200.0,1,200 m @ 10%,1
+49,-7.75,-0.887,2.7,200.0,0,200.0,1,200 m @ 10%,1
+50,-1.119,-1.012,2.7,200.0,0,200.0,1,200 m @ 10%,1
+51,7.757,-1.141,2.7,200.0,0,200.0,1,200 m @ 10%,1
+52,5.56,-1.266,2.7,200.0,0,200.0,1,200 m @ 10%,1
+53,3.34,-1.393,2.7,200.0,0,200.0,1,200 m @ 10%,1
+54,-3.347,-1.519,0.3,200.0,1,200.0,1,200 m @ 10%,1
+55,1.111,-1.646,2.7,200.0,0,200.0,1,200 m @ 10%,1
+56,-5.564,-1.773,2.7,200.0,0,200.0,1,200 m @ 10%,1
+57,-7.762,-1.901,2.7,200.0,0,200.0,1,200 m @ 10%,1
+58,-1.121,-2.027,2.7,200.0,0,200.0,1,200 m @ 10%,1
+59,7.768,-2.155,2.7,200.0,0,200.0,1,200 m @ 10%,1
+60,5.569,-2.282,2.7,200.0,0,200.0,1,200 m @ 10%,1
+61,3.345,-2.409,0.3,200.0,1,200.0,1,200 m @ 10%,1
+62,-3.353,-2.535,2.7,200.0,0,200.0,1,200 m @ 10%,1
+63,1.113,-2.663,2.7,200.0,0,200.0,1,200 m @ 10%,1
+64,-5.573,-2.789,2.7,200.0,0,200.0,1,200 m @ 10%,1
+65,-7.775,-2.916,2.7,200.0,0,200.0,1,200 m @ 10%,1
+66,-1.123,-3.044,2.7,200.0,0,140.0,0,200 m @ 37%,1
+67,7.78,-3.172,2.7,200.0,0,140.0,0,200 m @ 37%,1
+68,5.578,-3.299,0.3,200.0,1,140.0,0,200 m @ 37%,1
+69,3.351,-3.425,2.7,200.0,0,140.0,0,200 m @ 37%,1
+70,-3.358,-3.552,2.7,200.0,0,140.0,0,200 m @ 37%,1
+71,1.115,-3.68,2.7,200.0,0,140.0,0,200 m @ 37%,1
+72,-5.582,-3.806,2.7,200.0,0,140.0,0,200 m @ 37%,1
+73,-7.787,-3.933,2.7,200.0,0,140.0,0,200 m @ 37%,1
+74,-1.125,-4.062,2.7,200.0,0,140.0,0,200 m @ 37%,1
+75,7.792,-4.19,0.3,200.0,1,140.0,0,200 m @ 37%,1
+76,5.586,-4.318,2.7,200.0,0,140.0,0,200 m @ 37%,1
+77,3.356,-4.444,2.7,200.0,0,140.0,0,200 m @ 37%,1
+78,-3.363,-4.571,2.7,200.0,0,140.0,0,200 m @ 37%,1
+79,1.116,-4.699,2.7,200.0,0,140.0,0,200 m @ 37%,1
+80,-5.591,-4.824,2.7,200.0,0,140.0,0,200 m @ 37%,1
+81,-7.799,-4.951,2.7,200.0,0,140.0,0,200 m @ 37%,1
+82,-1.127,-5.081,0.3,200.0,1,140.0,0,200 m @ 37%,1
+83,7.804,-5.209,2.7,200.0,0,140.0,0,200 m @ 37%,1
+84,5.595,-5.336,2.7,200.0,0,140.0,0,200 m @ 37%,1
+85,3.36,-5.463,2.7,200.0,0,140.0,0,200 m @ 37%,1
+86,-3.369,-5.589,2.7,200.0,0,140.0,0,200 m @ 37%,1
+87,1.118,-5.718,2.7,200.0,0,140.0,0,200 m @ 37%,1
+88,-5.599,-5.843,2.7,200.0,0,140.0,0,200 m @ 37%,1
+89,-7.811,-5.968,2.7,200.0,0,140.0,0,200 m @ 37%,1
+90,-1.129,-6.1,0.3,100.0,1,140.0,0,100 m @ 3%,0
+91,-3.374,-6.607,2.7,100.0,0,140.0,0,100 m @ 3%,0
+92,-1.13,-7.117,2.7,100.0,0,140.0,0,100 m @ 3%,0
+93,-3.379,-7.624,0.3,100.0,1,140.0,0,100 m @ 3%,0
+94,-1.132,-8.134,2.7,100.0,0,140.0,0,100 m @ 3%,0
+95,-3.383,-8.64,2.7,100.0,0,140.0,0,100 m @ 3%,0
+96,3.381,-9.149,0.3,100.0,1,140.0,0,100 m @ 3%,0
+97,-3.388,-9.652,2.7,100.0,0,140.0,0,100 m @ 3%,0
+98,3.386,-10.16,2.7,100.0,0,100.0,0,100 m @ 10%,0
+99,1.129,-10.665,0.3,100.0,1,100.0,0,100 m @ 10%,0
+100,3.39,-11.17,2.7,100.0,0,100.0,0,100 m @ 10%,0
+101,1.129,-11.672,2.7,100.0,0,100.0,0,100 m @ 10%,0
+102,3.395,-12.174,0.3,100.0,1,100.0,0,100 m @ 10%,0
+103,1.131,-12.673,2.7,100.0,0,100.0,0,100 m @ 10%,0
+104,3.401,-13.173,2.7,100.0,0,100.0,0,100 m @ 10%,0
+105,1.133,-13.67,0.3,100.0,1,100.0,0,100 m @ 10%,0
+106,3.406,-14.166,2.7,100.0,0,50.0,0,100 m @ 120%,0
+107,1.135,-14.66,2.7,100.0,0,50.0,0,100 m @ 120%,0
+108,3.41,-15.154,0.3,100.0,1,50.0,0,100 m @ 120%,0
+109,1.137,-15.645,2.7,100.0,0,50.0,0,100 m @ 120%,0
+110,3.416,-16.135,2.7,100.0,0,50.0,0,100 m @ 120%,0
+111,1.139,-16.622,0.3,100.0,1,50.0,0,100 m @ 120%,0
+112,-1.142,-17.106,2.7,100.0,0,50.0,0,100 m @ 120%,0
+113,1.142,-17.592,2.7,100.0,0,50.0,0,100 m @ 120%,0
+114,-1.143,-18.072,0.3,100.0,1,50.0,0,100 m @ 120%,0
+115,-3.426,-18.548,2.7,100.0,0,50.0,0,100 m @ 120%,0
+116,-1.143,-19.03,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+117,-3.429,-19.501,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+118,-1.145,-19.978,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+119,-3.433,-20.445,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+120,-1.145,-20.918,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+121,-3.436,-21.379,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+122,-1.146,-21.848,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+123,-3.44,-22.304,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+124,-1.146,-22.768,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+125,-3.443,-23.219,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+126,-1.146,-23.678,0.3,100.0,1,25.0,0,100 m @ 1600%,0
+127,-3.446,-24.123,2.7,100.0,0,25.0,0,100 m @ 1600%,0
+128,-3.449,-25.016,0.3,100.0,1,25.0,0,100 m @ 1600%,0
diff --git a/ros2_ws/src/flyguard/flyguard/descending.py b/ros2_ws/src/flyguard/flyguard/descending.py
new file mode 100644
index 0000000..79707a2
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/descending.py
@@ -0,0 +1,219 @@
+"""DESCENDING NEURONS — решение.
+
+Весь разбор сцены у мухи сходится на нескольких десятках нисходящих нейронов,
+идущих в грудной ганглий. Два из них работают как раз по надвигающемуся объекту:
+
+* **Giant Fiber (DNp01)** — один толстый аксон с высоким порогом. Срабатывает
+ только на близкое и быстрое надвигание и запускает немедленный аварийный
+ взлёт, жертвуя устойчивостью ради скорости.
+* **DNp02/DNp11** — порог ниже, реакция раньше и мягче: муха успевает
+ подготовиться, не срываясь в паническое движение.
+
+Поезду нужна ровно такая же пара уровней: заблаговременное предупреждение с
+запасом по дальности и экстренное торможение по надёжному близкому объекту.
+Гистерезис здесь — не украшение: без него трек на пороге даёт дребезг, а
+дребезжащая команда торможения хуже её отсутствия.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from .central_complex import CentralComplex, Track
+
+BRAKING_DECEL = 1.0 # м/с², служебное торможение метропоезда (оценка)
+REACTION_TIME = 1.5 # с, задержка канала «решение → тормоз»
+
+
+@dataclass
+class DetectedObject:
+ distance: float
+ lateral: float
+ height: float
+ width: float
+ size_v: float
+ confidence: float
+ novelty: float
+ n_rays: int
+ track_id: int
+ ttc: float
+ # Смещение в системе лидара: `lateral` отсчитан от оси пути, и в кривой
+ # рамка по нему стоит в стороне от точек. NaN — неизвестно.
+ sensor_x: float = float("nan")
+
+
+@dataclass
+class Decision:
+ """Выход системы за один кадр."""
+
+ detected: bool = False
+ emergency: bool = False
+ distance: float = float("inf")
+ ttc: float = float("inf")
+ confidence: float = 0.0
+ stopping_distance: float = 0.0
+ objects: list[DetectedObject] = field(default_factory=list)
+ speed: float = 0.0
+
+ @property
+ def clear(self) -> bool:
+ return not self.detected
+
+
+class DescendingNeurons:
+ """Два порога с гистерезисом поверх подтверждённых треков."""
+
+ def __init__(self, *, warn_evidence: float = 0.5, clear_evidence: float = 0.3,
+ emergency_evidence: float = 0.75, min_hits: int = 2,
+ novelty_floor: float = 0.10, max_range: float = 200.0,
+ warn_far: float | None = None, warn_far_from: float = 90.0,
+ track_blend: float = 0.0, track_score: str = "w_mean",
+ track_gate: float = 0.0, track_readout=None,
+ far_channel: bool = False, far_confirm: int = 0,
+ far_confirm_from: float = 100.0):
+ self.warn_evidence = warn_evidence
+ self.clear_evidence = clear_evidence
+ self.emergency_evidence = emergency_evidence
+ self.min_hits = min_hits
+ self.novelty_floor = novelty_floor
+ self.max_range = max_range
+ # Порог предупреждения, ослабевающий с дальностью. Пара нисходящих
+ # нейронов у мухи не равноправна: DNp02/DNp11 срабатывают рано и
+ # мягко, Giant Fiber — поздно и жёстко. Здесь то же самое по
+ # дальности: далёкая улика физически слабее (4…8 лучей вместо
+ # сотни), а цена ошибки ниже — до предмета ещё сотня метров и
+ # десятки кадров на подтверждение. Экстренное торможение своего
+ # порога не снижает. None — послабления нет.
+ self.warn_far = warn_evidence if warn_far is None else warn_far
+ self.warn_far_from = warn_far_from
+ # Считывание по ТРЕКУ (см. track_readout.py). Улика — это сумма
+ # покадровых весов, и она ничего не знает ни о том, держится ли
+ # место в тоннеле, ни о том, растёт ли число лучей по закону
+ # решётки. Модель знает. Смешивание геометрическое, как и для
+ # покадрового считывания: 0 — решает только улика, ровно как
+ # раньше, 1 — только модель. Так размен меряется, а не объявляется.
+ self.track_blend = track_blend
+ # Жёсткий порог по тому же отсчёту — вместо смешивания или вместе с
+ # ним. Разница существенная: смешивание опускает отсчёт И предмету
+ # тоже (обе величины меньше единицы), а порог ничего не отнимает у
+ # того, кто его прошёл. На реальном объекте средний вес 0.998 у
+ # предмета против 0.269 у ложных треков — разделение, под которое
+ # порог и просится. 0 — не проверять.
+ self.track_gate = track_gate
+ # Дальний канал предупреждения (Zhirik1337, 22.09): далёкий трек с
+ # хорошими наблюдениями поднимает тревогу в обход порога, даже если
+ # улика просела на пропусках. Та же задача, что у `warn_far`, другим
+ # способом; включать вместе с ним — ослабить дальний край дважды.
+ # Замер — EXPERIMENTS п. 16.
+ self.far_channel = far_channel
+ # Дальний трек поднимает тревогу, только прожив `far_confirm` кадров.
+ # Вдали предмет — три-четыре луча, и по одному кадру он неотличим от
+ # вспышки на кромке или стыке; фантомы на синтетике и в записях жили
+ # 0.5–1 с и пропадали при подъезде. Торможению дальний трек не нужен
+ # срочно: до него сотня метров. 0 — выключено. EXPERIMENTS п. 19.
+ self.far_confirm = far_confirm
+ self.far_confirm_from = far_confirm_from
+ self.track_score = track_score
+ self.track_readout = track_readout
+ self._latched: set[int] = set()
+
+ @property
+ def _hyst(self) -> float:
+ return self.clear_evidence / max(self.warn_evidence, 1e-6)
+
+ def _warn_at(self, d: float) -> float:
+ """Порог тревоги на дальности d: линейно от warn до warn_far."""
+ span = self.max_range - self.warn_far_from
+ if span <= 0 or d <= self.warn_far_from:
+ return self.warn_evidence
+ f = min((d - self.warn_far_from) / span, 1.0)
+ return self.warn_evidence + (self.warn_far - self.warn_evidence) * f
+
+ def decide(self, cx: CentralComplex, speed: float) -> Decision:
+ out = Decision(speed=speed)
+ stop = speed * REACTION_TIME + speed * speed / (2 * BRAKING_DECEL)
+ out.stopping_distance = stop
+
+ # Дешёвые проверки сначала: модель считается только по тем трекам,
+ # которые их прошли, — обычно это единицы, а не полсотни.
+ ready: list[tuple[Track, float]] = []
+ for t in cx.tracks:
+ d = t.distance(cx.s_world)
+ far_floor = 0.05 if self.far_channel else self.novelty_floor
+ nov_ok = (t.novelty >= self.novelty_floor if d <= self.warn_far_from
+ else t.novelty >= far_floor)
+ if (not (0.0 < d <= self.max_range) or t.hits < self.min_hits
+ or not nov_ok):
+ self._latched.discard(t.id)
+ continue
+ if (self.far_confirm > 0 and d > self.far_confirm_from
+ and t.age < self.far_confirm and t.id not in self._latched):
+ continue
+ ready.append((t, d))
+
+ use_track = ((self.track_blend > 0.0 or self.track_gate > 0.0)
+ and bool(ready))
+ if use_track and self.track_score == "model":
+ if self.track_readout is None:
+ use_track = False
+ else:
+ self.track_readout.annotate_tracks([t for t, _ in ready],
+ cx.s_world)
+
+ live: list[tuple[Track, float]] = []
+ for t, d in ready:
+ warn = self._warn_at(d)
+ # гистерезис: попавший в тревогу трек держится до нижнего порога
+ on = warn if t.id not in self._latched else warn * self._hyst
+ score = t.evidence
+ if use_track:
+ q = (t.p_track if self.track_score == "model"
+ else t.p_mean if self.track_score == "p_mean"
+ else t.w_ema if self.track_score == "w_ema"
+ else t.w_mean)
+ if self.track_gate > 0.0 and q < self.track_gate:
+ self._latched.discard(t.id)
+ continue
+ if self.track_blend > 0.0:
+ b = min(max(self.track_blend, 0.0), 1.0)
+ score = (max(q, 1e-4) ** b
+ * max(t.evidence, 1e-4) ** (1.0 - b))
+
+ # Дальний канал раннего предупреждения (DNp02/DNp11):
+ # На дальности d > warn_far_from при наличии подтверждений (hits >= min_hits)
+ # трек с устойчивыми признаками (p_mean, w_mean, w_ema или p_track)
+ # квалифицируется как предупреждение, даже если улика просела из-за пропусков.
+ is_far = (self.far_channel and d > self.warn_far_from
+ and t.hits >= self.min_hits)
+ quality = max(t.p_mean, t.w_mean, t.w_ema)
+ far_qualified = is_far and (
+ (quality >= 0.40 and t.evidence >= 0.15)
+ or (getattr(t, "p_track", 0.0) >= 0.65)
+ )
+
+ if score < on and not far_qualified:
+ self._latched.discard(t.id)
+ continue
+ self._latched.add(t.id)
+ live.append((t, d))
+
+ if not live:
+ return out
+
+ live.sort(key=lambda p: p[1])
+ for t, d in live:
+ ttc = d / speed if speed > 0.5 else float("inf")
+ out.objects.append(DetectedObject(
+ distance=d, lateral=t.u, height=t.h, width=t.width, size_v=t.height,
+ confidence=min(1.0, t.evidence * t.novelty + 0.0),
+ novelty=t.novelty, n_rays=t.last_n_rays, track_id=t.id, ttc=ttc,
+ sensor_x=t.u_raw))
+
+ nearest, d0 = live[0]
+ out.detected = True
+ out.distance = d0
+ out.ttc = d0 / speed if speed > 0.5 else float("inf")
+ out.confidence = max(o.confidence for o in out.objects)
+ out.emergency = any(
+ t.evidence >= self.emergency_evidence and (d <= max(stop, 25.0))
+ for t, d in live)
+ return out
diff --git a/ros2_ws/src/flyguard/flyguard/device.py b/ros2_ws/src/flyguard/flyguard/device.py
new file mode 100644
index 0000000..b99dea3
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/device.py
@@ -0,0 +1,150 @@
+"""Управление вычислительными устройствами (NVIDIA GPU / CUDA и CPU fallback).
+
+Модуль обеспечивает прозрачную работу конвейера FlyGuard на:
+- Выделенном NVIDIA GPU (RTX 4070 Ti Super 16GB / Ada Lovelace) через CUDA / Tensor Cores.
+- Любых других видеокартах NVIDIA (Ampere, Turing, Pascal, Hopper, Blackwell).
+- CPU Linux при отсутствии GPU или в среде жюри (Graceful Fallback).
+"""
+from __future__ import annotations
+
+import logging
+from typing import Any
+
+import numpy as np
+
+logger = logging.getLogger("flyguard.device")
+
+_TORCH_AVAILABLE: bool | None = None
+_CUDA_AVAILABLE: bool | None = None
+
+
+def is_torch_available() -> bool:
+ """Проверка наличия установленного пакета PyTorch."""
+ global _TORCH_AVAILABLE
+ if _TORCH_AVAILABLE is None:
+ try:
+ import torch # noqa: F401
+ _TORCH_AVAILABLE = True
+ except ImportError:
+ _TORCH_AVAILABLE = False
+ return _TORCH_AVAILABLE
+
+
+def is_cuda_available() -> bool:
+ """Проверка доступности ускорения NVIDIA CUDA."""
+ global _CUDA_AVAILABLE
+ if _CUDA_AVAILABLE is None:
+ if not is_torch_available():
+ _CUDA_AVAILABLE = False
+ else:
+ try:
+ import torch
+ _CUDA_AVAILABLE = bool(torch.cuda.is_available() and torch.cuda.device_count() > 0)
+ except Exception as e:
+ logger.warning("Проверка CUDA завершилась ошибкой: %s. Fallback на CPU.", e)
+ _CUDA_AVAILABLE = False
+ return _CUDA_AVAILABLE
+
+
+def notify_cuda_error(exc: Exception | None = None) -> None:
+ """Зафиксировать сбой CUDA в рантайме и принудительно перевести систему в режим CPU fallback.
+
+ Вызывается, если во время работы на GPU произошёл OOM, таймаут или сбой драйвера.
+ Последующие вызовы конвейера будут прозрачно исполняться на CPU.
+ """
+ global _CUDA_AVAILABLE
+ _CUDA_AVAILABLE = False
+ logger.warning("Зафиксирован сбой GPU в рантайме (%s). Выполнен необратимый Fallback на CPU.", exc)
+
+
+def reset_device_cache() -> None:
+ """Сбросить кэш состояния устройств (для юнит-тестов)."""
+ global _TORCH_AVAILABLE, _CUDA_AVAILABLE
+ _TORCH_AVAILABLE = None
+ _CUDA_AVAILABLE = None
+
+
+def get_device(preferred: str = "auto") -> str:
+ """Выбрать вычислительное устройство с автоматическим fallback на CPU.
+
+ Args:
+ preferred: 'auto' (выбрать cuda при наличии, иначе cpu), 'cuda', 'cuda:0', 'cpu'.
+
+ Returns:
+ Строка устройства: 'cuda', 'cuda:X' или 'cpu'.
+ """
+ pref = (preferred or "auto").strip().lower()
+ if pref == "auto":
+ return "cuda" if is_cuda_available() else "cpu"
+ if pref.startswith("cuda"):
+ if is_cuda_available():
+ return pref
+ logger.warning("Запрошено устройство '%s', но CUDA недоступна. Выполнен fallback на CPU.", pref)
+ return "cpu"
+ return "cpu"
+
+
+def get_device_info(device_str: str | None = None) -> dict[str, Any]:
+ """Сводная информация об аппаратных ресурсах для инспекции и логирования."""
+ dev = get_device(device_str or "auto")
+ info: dict[str, Any] = {
+ "device": dev,
+ "torch_available": is_torch_available(),
+ "cuda_available": is_cuda_available(),
+ }
+ if is_torch_available() and is_cuda_available() and dev.startswith("cuda"):
+ import torch
+ dev_idx = 0
+ if ":" in dev:
+ try:
+ dev_idx = int(dev.split(":")[1])
+ except ValueError:
+ dev_idx = 0
+ props = torch.cuda.get_device_properties(dev_idx)
+ info.update({
+ "name": props.name,
+ "total_memory_mb": round(props.total_memory / (1024 * 1024), 2),
+ "major": props.major,
+ "minor": props.minor,
+ "multi_processor_count": props.multi_processor_count,
+ "allocated_mb": round(torch.cuda.memory_allocated(dev_idx) / (1024 * 1024), 2),
+ "reserved_mb": round(torch.cuda.memory_reserved(dev_idx) / (1024 * 1024), 2),
+ "cuda_version": torch.version.cuda,
+ "cudnn_version": torch.backends.cudnn.version() if torch.backends.cudnn.is_available() else None,
+ })
+ else:
+ import platform
+ info.update({
+ "name": platform.processor() or "CPU",
+ "python_version": platform.python_version(),
+ })
+ return info
+
+
+def log_device_info(device_str: str | None = None) -> None:
+ """Вывести в лог/консоль активное вычислительное окружение."""
+ info = get_device_info(device_str)
+ if info.get("cuda_available") and str(info["device"]).startswith("cuda"):
+ logger.info("FlyGuard GPU Active: %s (VRAM: %.1f MB, CUDA: %s, SM: %d.%d)",
+ info.get("name"), info.get("total_memory_mb", 0.0),
+ info.get("cuda_version"), info.get("major", 0), info.get("minor", 0))
+ else:
+ logger.info("FlyGuard CPU Mode (Fallback): %s", info.get("name"))
+
+
+def empty_cache() -> None:
+ """Очистить кэш памяти CUDA для предотвращения фрагментации VRAM."""
+ if is_cuda_available():
+ import torch
+ torch.cuda.empty_cache()
+
+
+def to_numpy(arr: Any) -> np.ndarray:
+ """Преобразовать входной тензор (PyTorch или NumPy) в numpy.ndarray."""
+ if isinstance(arr, np.ndarray):
+ return arr
+ if is_torch_available():
+ import torch
+ if isinstance(arr, torch.Tensor):
+ return arr.detach().cpu().numpy()
+ return np.asarray(arr)
diff --git a/ros2_ws/src/flyguard/flyguard/export.py b/ros2_ws/src/flyguard/flyguard/export.py
new file mode 100644
index 0000000..f6d7e7c
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/export.py
@@ -0,0 +1,303 @@
+"""ROS2 и визуализационный экспорт решений FlyGuard.
+
+Преобразует внутренние результаты конвейера (Track, Decision, RailPlane, Corridor)
+в стандартизованные 3D Bounding Boxes, вектор угроз и структуры MarkerArray для RViz.
+
+Работает автономно на чистом Python + NumPy, не требуя обязательной установки
+библиотек rclpy / ros2 на стенде валидации. При наличии ROS2 может конвертировать
+напрямую в сообщения visualization_msgs и vision_msgs.
+"""
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass, field
+from enum import IntEnum
+import math
+import numpy as np
+
+from .descending import Decision, DetectedObject
+from .geometry import Corridor, RailPlane
+
+
+class ThreatLevel(IntEnum):
+ """Уровень опасности для системы автоведения поезда."""
+ CLEAR = 0 # Путь свободен
+ WARNING = 1 # Заблаговременное предупреждение (DNp02/DNp11 soft-warning)
+ EMERGENCY = 2 # Экстренное торможение (DNp01 / Giant Fiber)
+
+
+@dataclass
+class BoundingBox3D:
+ """3D ориентированный параллелепипед в координатах сенсора лидара."""
+
+ # Центр бокса в системе сенсора (x: вправо, y: вперёд (-d), z: вверх)
+ x: float
+ y: float
+ z: float
+
+ # Размеры бокса (м)
+ dx: float # ширина поперёк пути
+ dy: float # протяжённость вдоль пути
+ dz: float # высота
+
+ # Ориентация (рыскание относительно оси лидара, рад)
+ yaw: float
+
+ # Метрики движения и трекинга
+ distance_along_track: float
+ lateral_offset: float
+ height_above_rail: float
+ confidence: float
+ novelty: float
+ ttc: float
+ track_id: int
+ threat_level: ThreatLevel
+
+ def to_dict(self) -> dict:
+ d = asdict(self)
+ d["threat_level"] = self.threat_level.name
+ return d
+
+
+@dataclass
+class ExportResult:
+ """Полный экспортный пакет за один кадр."""
+
+ stamp: float
+ threat_level: ThreatLevel
+ nearest_distance: float
+ ttc: float
+ stopping_distance: float
+ speed_mps: float
+ speed_kmh: float
+ boxes: list[BoundingBox3D] = field(default_factory=list)
+ corridor_points_xyz: list[tuple[float, float, float]] = field(default_factory=list)
+
+ def to_dict(self) -> dict:
+ return {
+ "stamp": self.stamp,
+ "threat_level": self.threat_level.name,
+ "threat_code": int(self.threat_level),
+ "nearest_distance": self.nearest_distance,
+ "ttc": self.ttc,
+ "stopping_distance": self.stopping_distance,
+ "speed_kmh": self.speed_kmh,
+ "n_objects": len(self.boxes),
+ "boxes": [b.to_dict() for b in self.boxes],
+ "corridor_points": self.corridor_points_xyz,
+ }
+
+ def to_rviz_markers(self, frame_id: str = "hesai_pandar") -> list[dict]:
+ """Генерация словарей, готовых для преобразования в visualization_msgs/Marker."""
+ markers = []
+ now_sec = int(self.stamp)
+ now_nanosec = int((self.stamp - now_sec) * 1e9)
+
+ # 1. Линия коридора пути (LINE_STRIP, type 4)
+ if self.corridor_points_xyz:
+ markers.append({
+ "header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
+ "ns": "flyguard_corridor",
+ "id": 0,
+ "type": 4, # LINE_STRIP
+ "action": 0, # ADD
+ "scale": {"x": 0.12},
+ "color": {"r": 0.2, "g": 0.8, "b": 1.0, "a": 0.8},
+ "points": [{"x": p[0], "y": p[1], "z": p[2]} for p in self.corridor_points_xyz]
+ })
+
+ # 2. Bounding boxes объектов (CUBE, type 1) и надписи (TEXT, type 9)
+ for i, box in enumerate(self.boxes):
+ if box.threat_level == ThreatLevel.EMERGENCY:
+ color = {"r": 1.0, "g": 0.1, "b": 0.1, "a": 0.75} # Красный
+ elif box.threat_level == ThreatLevel.WARNING:
+ color = {"r": 1.0, "g": 0.85, "b": 0.0, "a": 0.65} # Жёлтый
+ else:
+ color = {"r": 0.2, "g": 0.8, "b": 0.2, "a": 0.50} # Зелёный
+
+ # Кватернион поворота вокруг оси Z (yaw)
+ cy = math.cos(box.yaw * 0.5)
+ sy = math.sin(box.yaw * 0.5)
+
+ # CUBE маркер
+ markers.append({
+ "header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
+ "ns": "flyguard_bboxes",
+ "id": box.track_id * 2,
+ "type": 1, # CUBE
+ "action": 0,
+ "pose": {
+ "position": {"x": box.x, "y": box.y, "z": box.z},
+ "orientation": {"x": 0.0, "y": 0.0, "z": sy, "w": cy}
+ },
+ "scale": {"x": max(box.dx, 0.2), "y": max(box.dy, 0.2), "z": max(box.dz, 0.2)},
+ "color": color
+ })
+
+ # TEXT_VIEW_FACING над объектом
+ ttc_str = f"{box.ttc:.1f}s" if math.isfinite(box.ttc) else "inf"
+ label = f"ID:{box.track_id} | {box.distance_along_track:.1f}m | TTC:{ttc_str}"
+ markers.append({
+ "header": {"frame_id": frame_id, "sec": now_sec, "nanosec": now_nanosec},
+ "ns": "flyguard_labels",
+ "id": box.track_id * 2 + 1,
+ "type": 9, # TEXT_VIEW_FACING
+ "action": 0,
+ "pose": {
+ "position": {"x": box.x, "y": box.y, "z": box.z + box.dz * 0.5 + 0.35},
+ "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+ },
+ "scale": {"z": 0.40}, # Высота шрифта
+ "color": {"r": 1.0, "g": 1.0, "b": 1.0, "a": 0.95},
+ "text": label
+ })
+
+ return markers
+
+
+def export_frame(decision: Decision, plane: RailPlane | None, corridor: Corridor | None,
+ stamp: float = 0.0) -> ExportResult:
+ """Сконвертировать решение FlyGuard в экспортный формат.
+
+ Parameters
+ ----------
+ decision : Decision
+ Итоговый вердикт системы за кадр.
+ plane : RailPlane, optional
+ Плоскость головок рельсов (z = a·d + b·u + c).
+ corridor : Corridor, optional
+ Осевая линия тоннеля (парабола u(d)).
+ stamp : float
+ Временная метка кадра.
+ """
+ if decision.emergency:
+ threat = ThreatLevel.EMERGENCY
+ elif decision.detected:
+ threat = ThreatLevel.WARNING
+ else:
+ threat = ThreatLevel.CLEAR
+
+ # Параметры плоскости пути: z = a*d + b*u + c
+ a = plane.a if plane is not None else 0.0
+ b = plane.b if plane is not None else 0.0
+ c = plane.c if plane is not None else -1.80 # высота лидара над рельсами ~1.8м
+
+ boxes: list[BoundingBox3D] = []
+ for obj in decision.objects:
+ d = obj.distance
+ u = obj.lateral
+ h = obj.height
+
+ # Координаты в системе сенсора (x: вправо, y: вперёд (-d), z: вверх).
+ #
+ # Боковое смещение трека отсчитано от ОСИ ПУТИ, а не от оси сенсора
+ # (TrackFrame.lateral), поэтому в кривой к нему прибавляется положение
+ # оси на этой дальности: при радиусе 1300 м это 1.2 м на 55 м и 8.6 м
+ # на 150 м — без поправки рамка рисовалась в стене. Направление оси
+ # пути в кадре — (наклон, −1), и длинная ось рамки (её локальная y)
+ # совпадает с ним при повороте на +atan(наклон). Проверка обоих —
+ # test_export_box_follows_a_curved_track.
+ #
+ # Если трек знает своё смещение в системе лидара (`sensor_x`), берётся
+ # оно: габарит — объединение прямого и изогнутого, и у предмета,
+ # попавшего в прямой, `u` отсчитан не от кривой, а от оси лидара.
+ sx = float(getattr(obj, "sensor_x", float("nan")))
+ if corridor is not None and corridor.n_slices > 0:
+ x_sensor = (sx if math.isfinite(sx)
+ else float(u + corridor.centre(np.array([d], np.float32))[0]))
+ c0, c1, c2 = corridor.coef
+ dm = max(corridor.d_max_seen, 1.0)
+ d_in = min(d, dm)
+ slope = c1 + 2.0 * c2 * d_in
+ yaw = float(math.atan(slope))
+ else:
+ x_sensor = sx if math.isfinite(sx) else float(u)
+ yaw = 0.0
+ y_sensor = float(-d)
+ z_sensor = float(h + a * d + b * x_sensor + c)
+
+ # Уровень опасности для конкретного объекта
+ if decision.emergency and d <= max(decision.stopping_distance, 25.0):
+ obj_threat = ThreatLevel.EMERGENCY
+ elif decision.detected:
+ obj_threat = ThreatLevel.WARNING
+ else:
+ obj_threat = ThreatLevel.CLEAR
+
+ # Размеры: dx поперёк пути, dy вдоль пути, dz по вертикали
+ dx = float(max(obj.width, 0.35))
+ # протяжённость вдоль пути трек не хранит, поэтому она постоянная
+ dy = 0.50
+ dz = float(max(obj.size_v, 0.40))
+
+ boxes.append(BoundingBox3D(
+ x=x_sensor,
+ y=y_sensor,
+ z=z_sensor,
+ dx=dx,
+ dy=dy,
+ dz=dz,
+ yaw=yaw,
+ distance_along_track=float(d),
+ lateral_offset=float(u),
+ height_above_rail=float(h),
+ confidence=float(obj.confidence),
+ novelty=float(obj.novelty),
+ ttc=float(obj.ttc),
+ track_id=int(obj.track_id),
+ threat_level=obj_threat,
+ ))
+
+ # Траектория коридора вперед (на 10..180 м)
+ corridor_pts: list[tuple[float, float, float]] = []
+ if corridor is not None:
+ ds_sample = np.linspace(10.0, min(max(corridor.d_max_seen, 50.0), 180.0), 25)
+ us_sample = corridor.centre(ds_sample)
+ for ds_i, us_i in zip(ds_sample, us_sample):
+ xs = float(us_i)
+ ys = float(-ds_i)
+ zs = float(a * ds_i + b * us_i + c + 0.1) # чуть над рельсом
+ corridor_pts.append((xs, ys, zs))
+
+ speed = decision.speed
+ return ExportResult(
+ stamp=stamp,
+ threat_level=threat,
+ nearest_distance=float(decision.distance),
+ ttc=float(decision.ttc),
+ stopping_distance=float(decision.stopping_distance),
+ speed_mps=float(speed),
+ speed_kmh=float(speed * 3.6),
+ boxes=boxes,
+ corridor_points_xyz=corridor_pts
+ )
+
+
+def gauge_outline(boxes, centre=None, step: float = 2.0,
+ frame_every: float = 20.0) -> list[tuple[float, float, float]]:
+ """Отрезки контура габарита (пары точек для LINE_LIST) в координатах облака обзора.
+
+ `boxes` — секции `(полуширина, низ, верх, от, до)`; `centre(d)` — ось пути
+ (`Corridor.centre`), None — прямой короб вдоль x = 0. Облако обзора в кривой
+ не выпрямлено, поэтому габарит вдоль изогнутой оси рисуется изогнутым —
+ там, где узел его и проверяет. Вперёд = −y, вправо = +x, вверх = +z над
+ головкой рельса, как у облака обзора. Рамки поперёк — через `frame_every`.
+ """
+ pts: list[tuple[float, float, float]] = []
+ for hw, z0, z1, d0, d1 in boxes:
+ if d1 <= d0:
+ continue
+ n = 1 if centre is None else max(int(np.ceil((d1 - d0) / step)), 1)
+ ds = np.linspace(d0, d1, n + 1)
+ cx = np.zeros_like(ds) if centre is None else np.asarray(centre(ds), np.float64)
+ corners = [(-hw, z0), (hw, z0), (hw, z1), (-hw, z1)]
+ for x, z in corners: # вдоль пути
+ for a in range(n):
+ pts.append((float(cx[a] + x), float(-ds[a]), float(z)))
+ pts.append((float(cx[a + 1] + x), float(-ds[a + 1]), float(z)))
+ fd = np.arange(d0, d1 + 1e-6, frame_every) # поперёк
+ fc = np.zeros_like(fd) if centre is None else np.asarray(centre(fd), np.float64)
+ for dd, c in zip(fd, fc):
+ for (xa, za), (xb, zb) in zip(corners, corners[1:] + corners[:1]):
+ pts.append((float(c + xa), float(-dd), float(za)))
+ pts.append((float(c + xb), float(-dd), float(zb)))
+ return pts
diff --git a/ros2_ws/src/flyguard/flyguard/fan_body.py b/ros2_ws/src/flyguard/flyguard/fan_body.py
new file mode 100644
index 0000000..32133ba
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/fan_body.py
@@ -0,0 +1,180 @@
+"""FAN-SHAPED BODY — накопление слабых улик в координатах пути.
+
+Покадровое решение проваливается там, где предмет даёт единицы лучей. На 170 м
+человек освещён **в каждом** кадре, но всего пятью лучами: формы из них не
+построить, а локальный контраст на такой дальности обнуляется — кольцо
+окружения ламины упирается в стену тоннеля, которая на той же дальности.
+Зато предмет неподвижен в мире, а тоннель проплывает мимо: за проход одни и те
+же пять лучей попадают в одну и ту же точку пространства семнадцать раз подряд.
+
+Именно так устроено веерное тело центрального комплекса мухи: оно копит
+вектор к цели в координатах, привязанных к миру, а не к текущему кадру, и
+достаёт из слабого повторяющегося сигнала то, чего нет ни в одном отдельном
+наблюдении.
+
+Здесь то же самое буквально: сетка, привязанная к пройденному пути, в которую
+кадр за кадром складываются лучи из габарита. Сетка сдвигается на пройденное
+расстояние, поэтому неподвижный предмет всегда попадает в одну ячейку, а шум
+и случайные отражения размазываются.
+
+Своих кандидатов накопитель не порождает — это проверено и отвергнуто
+измерением: по геометрии накопленное скопление предмета и накопленный кусок
+конструкции тоннеля неразличимы (совпадают все десять признаков, от дальности
+до протяжённости). Разделяет их только память тоннеля, а ей нужны признаки
+кадра, которых у скопления нет.
+
+Роль накопителя другая и точная: он отвечает про **уже найденного покадрового
+кандидата**, возвращались ли лучи из этой точки мира кадр за кадром. На
+больших дальностях это единственная доступная улика — локальный контраст там
+структурно равен нулю, потому что кольцо окружения ламины упирается в стену
+тоннеля, стоящую на той же дальности.
+
+Стоимость — доли миллисекунды: пара тысяч лучей в `bincount` по сетке из
+пятнадцати тысяч ячеек.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy import ndimage
+
+
+# Шаг сетки вдоль пути крупный намеренно: оценка собственного движения копит
+# ошибку около 3 %, за семнадцать кадров это уже 0.7 м. Мелкая ячейка размазала
+# бы предмет по соседям и убила весь смысл накопления. Для отчёта о дистанции
+# 2 м всё равно на порядок точнее допуска (12 % дальности).
+DS_BIN = 2.0 # м вдоль пути
+DU_BIN = 0.20 # м поперёк
+DH_BIN = 0.25 # м по высоте
+
+DECAY = 0.97 # забывание: полураспад около 23 кадров
+MIN_FRAMES = 6 # меньше — это вспышка, а не предмет
+MAX_SPAN_S = 8.0 # м: длиннее — это стена или лоток, а не предмет
+MAX_WIDTH = 1.5 # м: шире — край габарита, а не предмет
+BG_WIN_M = 24.0 # м: окно оценки фона вдоль пути
+
+
+@dataclass
+class Accumulated:
+ """Скопление попаданий в одной точке пути."""
+
+ d: float
+ u: float
+ h: float
+ h_min: float
+ width: float
+ height: float
+ span_s: float
+ hits: float
+ frames: float
+ support: float # попаданий относительно ожидаемого для этой дальности
+
+
+class FanBody:
+ """Сетка в координатах пути, копящая лучи из габарита."""
+
+ def __init__(self, *, d_max: float = 220.0, half_width: float = 1.6,
+ h_lo: float = 0.28, h_hi: float = 2.3,
+ d_near: float = 55.0, min_support: float = 0.8,
+ ref_rays: float = 1.4, ref_d: float = 175.0):
+ self.ds, self.du, self.dh = DS_BIN, DU_BIN, DH_BIN
+ self.h_lo, self.h_hi = h_lo, h_hi
+ self.half_width = half_width
+ self.d_near = d_near # ближе этого покадровый тракт и так справляется
+ self.min_support = min_support
+ # Сколько попаданий ждать от настоящего предмета на данной дальности.
+ # Калибровано по замеру: человек, вставленный в реальный проезд, даёт
+ # после вычитания фона и разброса по ячейкам эквивалент 1.4 луча в
+ # кадре на 175 м (пять лучей приходят, но часть уходит в фон и в
+ # соседние ячейки). Число лучей падает как 1/R², накопление держит
+ # около 1/(1−DECAY) кадров.
+ self.k_expect = ref_rays * ref_d ** 2 / (1.0 - DECAY)
+
+ self.n_s = int(np.ceil(d_max / self.ds)) + 1
+ self.n_u = int(np.ceil(2 * half_width / self.du)) + 1
+ self.n_h = int(np.ceil((h_hi - h_lo) / self.dh)) + 1
+ self.hits = np.zeros((self.n_s, self.n_u, self.n_h), np.float32)
+ self.seen = np.zeros((self.n_s, self.n_u), np.float32)
+ self._off = 0.0 # смещение начала сетки внутри ячейки, м
+
+ # ------------------------------------------------------------------ такт
+
+ def update(self, d: np.ndarray, u: np.ndarray, h: np.ndarray, ds: float) -> None:
+ """Сдвинуть сетку на пройденное `ds` и досыпать лучи текущего кадра."""
+ self._advance(ds)
+ if d.size == 0:
+ return
+ si = ((self._off + d) / self.ds).astype(np.int32)
+ ui = ((u + self.half_width) / self.du).astype(np.int32)
+ hi = ((h - self.h_lo) / self.dh).astype(np.int32)
+ ok = ((si >= 0) & (si < self.n_s) & (ui >= 0) & (ui < self.n_u)
+ & (hi >= 0) & (hi < self.n_h))
+ if not ok.any():
+ return
+ flat = (si[ok] * self.n_u + ui[ok]) * self.n_h + hi[ok]
+ self.hits += np.bincount(flat, minlength=self.hits.size).reshape(self.hits.shape)
+ # «Кадров подряд» считается по столбцу, а не по ячейке высоты: предмет
+ # может качнуться на четверть метра, и это не повод обнулять счёт.
+ col = np.unique(si[ok] * self.n_u + ui[ok])
+ self.seen.ravel()[col] += 1.0
+
+ def _advance(self, ds: float) -> None:
+ self.hits *= DECAY
+ self.seen *= DECAY
+ if ds <= 0:
+ return
+ self._off += ds
+ k = int(self._off // self.ds)
+ if k <= 0:
+ return
+ self._off -= k * self.ds
+ if k >= self.n_s:
+ self.hits[:] = 0.0
+ self.seen[:] = 0.0
+ return
+ self.hits[:-k] = self.hits[k:]
+ self.hits[-k:] = 0.0
+ self.seen[:-k] = self.seen[k:]
+ self.seen[-k:] = 0.0
+
+ # ------------------------------------------------------------------ съём
+
+ def support_at(self, d: np.ndarray, u: np.ndarray) -> np.ndarray:
+ """Опора накопителя в точках пути: во сколько раз попаданий больше фона.
+
+ Своих кандидатов накопитель не порождает намеренно. Замер показал, что
+ по геометрии накопленное скопление предмета и накопленный кусок
+ конструкции тоннеля неразличимы: совпадают дальность, смещение, высота,
+ ширина, протяжённость — всё. Разделяет их только память тоннеля, а она
+ работает с признаками кадра (контраст, интенсивность, тень), которых у
+ скопления нет.
+
+ Поэтому накопитель отвечает на один вопрос про уже найденного
+ покадрового кандидата: **возвращались ли лучи из этой точки мира кадр
+ за кадром**. На больших дальностях это единственная доступная улика:
+ локальный контраст там структурно равен нулю, потому что кольцо
+ окружения упирается в стену тоннеля на той же дальности.
+ """
+ if d.size == 0:
+ return np.zeros(0, np.float32)
+ col = self.hits.sum(axis=2)
+ win = max(int(BG_WIN_M / self.ds) | 1, 3)
+ base = ndimage.median_filter(col, size=(win, 1), mode="nearest")
+ resid = np.maximum(col - base, 0.0)
+
+ si = np.clip(((self._off + d) / self.ds).astype(np.int32), 0, self.n_s - 1)
+ ui = np.clip(((u + self.half_width) / self.du).astype(np.int32), 0, self.n_u - 1)
+ # окно ±1 ячейка: предмет шире одной ячейки, а оценка пути слегка плывёт
+ tot = np.zeros(d.size, np.float32)
+ frames = np.zeros(d.size, np.float32)
+ for ds_ in (-1, 0, 1):
+ for du_ in (-1, 0, 1):
+ a = np.clip(si + ds_, 0, self.n_s - 1)
+ b = np.clip(ui + du_, 0, self.n_u - 1)
+ tot += resid[a, b]
+ frames = np.maximum(frames, self.seen[a, b])
+ expect = self.k_expect / np.maximum(d, 1.0) ** 2
+ sup = tot / np.maximum(expect, 1e-6)
+ # без нескольких кадров подряд это не улика, а вспышка
+ return np.where(frames > MIN_FRAMES, sup, 0.0).astype(np.float32)
diff --git a/ros2_ws/src/flyguard/flyguard/geometry.py b/ros2_ws/src/flyguard/flyguard/geometry.py
new file mode 100644
index 0000000..506bed4
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/geometry.py
@@ -0,0 +1,324 @@
+"""Система координат пути, плоскость рельсов и «ожидаемая дальность до пола».
+
+Соответствие мухе — **жужжальца и оцеллии**. Прежде чем обрабатывать изображение,
+муха стабилизирует взгляд: жужжальца дают угловые скорости, оцеллии — направление
+на горизонт, и голова доворачивается так, чтобы зрительный мир не «плавал».
+Здесь роль горизонта играет плоскость пути: она оценивается по самим данным
+в каждом кадре, поэтому крепление сенсора не обязано быть жёстким, а качка
+вагона не превращается в ложные срабатывания.
+
+Ключевая величина дальше по конвейеру — **ожидаемая дальность до пола** для
+каждого луча. Луч с отрицательной элевацией, если ему ничто не мешает, обязан
+закончиться на плоскости пути на строго определённом расстоянии. Всё, что
+обрывает его раньше, — предмет, стоящий на пути. Это даёт детектор, не зависящий
+от абсолютного размера объекта и работающий на любой дальности.
+
+Система координат пути (используется во всём проекте):
+ d — вперёд по ходу движения, м (в кадре сенсора это −y)
+ u — поперёк, вправо, м (в кадре сенсора это x)
+ h — вверх от плоскости рельсов, м
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .retina import RangeImage, ScanLayout
+
+
+@dataclass(frozen=True)
+class RailPlane:
+ """Плоскость головок рельсов в системе сенсора: z = a·d + b·u + c."""
+
+ a: float # тангаж: подъём плоскости с расстоянием
+ b: float # крен: наклон плоскости поперёк
+ c: float # −высота сенсора над путём (c < 0)
+ inliers: int
+ rms: float
+
+ @property
+ def height(self) -> float:
+ """Высота сенсора над головкой рельса, м."""
+ return -self.c
+
+ @property
+ def pitch_deg(self) -> float:
+ return float(np.degrees(np.arctan(self.a)))
+
+ @property
+ def roll_deg(self) -> float:
+ return float(np.degrees(np.arctan(self.b)))
+
+ def height_of(self, d: np.ndarray, u: np.ndarray, z: np.ndarray) -> np.ndarray:
+ """Высота точек над плоскостью пути."""
+ return z - (self.a * d + self.b * u + self.c)
+
+ def floor_range(self, layout: ScanLayout) -> np.ndarray:
+ """Дальность, на которой каждый луч упёрся бы в плоскость пути.
+
+ Луч r·(dx, dy, dz); подстановка в уравнение плоскости даёт
+ r = c / (dz + a·dy − b·dx). Лучи, уходящие вверх или параллельно
+ плоскости, получают +inf.
+ """
+ dx = layout.dirs[..., 0]
+ dy = layout.dirs[..., 1]
+ dz = layout.dirs[..., 2]
+ denom = dz + self.a * dy - self.b * dx
+ with np.errstate(divide="ignore", invalid="ignore"):
+ r = self.c / denom
+ return np.where((denom < -1e-6) & np.isfinite(r), r, np.float32(np.inf)).astype(np.float32)
+
+
+DEFAULT_PLANE = RailPlane(a=0.0, b=0.0, c=-2.19, inliers=0, rms=0.0)
+
+
+def fit_rail_plane(img: RangeImage, layout: ScanLayout, *,
+ d_min: float = 6.0, d_max: float = 45.0,
+ u_max: float = 1.9, cell_d: float = 1.0, cell_u: float = 0.25,
+ iters: int = 4, prev: RailPlane | None = None,
+ smooth: float = 0.25) -> RailPlane:
+ """Робастная оценка плоскости пути по ближней зоне.
+
+ В каждой ячейке сетки (d, u) остаётся только самая низкая точка — это
+ отсекает шпалы, кабельные лотки и всё, что стоит на полотне. Затем идут
+ итерации перевзвешенных наименьших квадратов с мягкой функцией Хьюбера,
+ после чего подгонка повторяется уже только по точкам у самой плоскости.
+
+ `smooth` задаёт постоянную времени экспоненциального сглаживания по кадрам:
+ плоскость пути физически не может прыгать, и сглаживание играет ту же роль,
+ что обратная связь от жужжалец, — гасит дрожание оценки.
+ """
+ xyz = img.xyz(layout)
+ x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
+ d = -y
+ sel = img.valid & (d > d_min) & (d < d_max) & (np.abs(x) < u_max)
+ if sel.sum() < 200:
+ return prev or DEFAULT_PLANE
+
+ dv = d[sel].astype(np.float64)
+ uv = x[sel].astype(np.float64)
+ zv = z[sel].astype(np.float64)
+
+ # самая низкая точка в каждой ячейке — грубое выделение полотна
+ ci = ((dv - d_min) / cell_d).astype(np.int64)
+ cj = ((uv + u_max) / cell_u).astype(np.int64)
+ key = ci * 10_000 + cj
+ order = np.lexsort((zv, key))
+ key_s = key[order]
+ first = np.ones(key_s.size, bool)
+ first[1:] = key_s[1:] != key_s[:-1]
+ idx = order[first]
+ if idx.size < 40:
+ return prev or DEFAULT_PLANE
+
+ dd, uu, zz = dv[idx], uv[idx], zv[idx]
+ coef = _irls_plane(dd, uu, zz, iters)
+ if coef is None:
+ return prev or DEFAULT_PLANE
+
+ # второй проход: только точки у найденной плоскости, уже без отбора минимумов
+ res_all = zv - (coef[0] * dv + coef[1] * uv + coef[2])
+ near = np.abs(res_all) < 0.18
+ if near.sum() > 300:
+ c2 = _irls_plane(dv[near], uv[near], zv[near], iters)
+ if c2 is not None:
+ coef = c2
+
+ res = zv - (coef[0] * dv + coef[1] * uv + coef[2])
+ keep = np.abs(res) < 0.2
+ plane = RailPlane(a=float(coef[0]), b=float(coef[1]), c=float(coef[2]),
+ inliers=int(keep.sum()),
+ rms=float(np.sqrt(np.mean(res[keep] ** 2))) if keep.any() else 9.9)
+
+ # защита от вырождения: высота сенсора над путём физически ограничена
+ if not (0.5 < plane.height < 5.0) or abs(plane.pitch_deg) > 8 or abs(plane.roll_deg) > 8:
+ return prev or DEFAULT_PLANE
+ if prev is not None and smooth > 0:
+ k = smooth
+ plane = RailPlane(a=k * plane.a + (1 - k) * prev.a,
+ b=k * plane.b + (1 - k) * prev.b,
+ c=k * plane.c + (1 - k) * prev.c,
+ inliers=plane.inliers, rms=plane.rms)
+ return plane
+
+
+def _irls_plane(d: np.ndarray, u: np.ndarray, z: np.ndarray, iters: int):
+ """z ≈ a·d + b·u + c с мягким Хьюбером."""
+ A = np.stack([d, u, np.ones_like(d)], axis=1)
+ w = np.ones_like(z)
+ coef = np.array([0.0, 0.0, float(np.median(z))])
+ for _ in range(iters):
+ try:
+ coef, *_ = np.linalg.lstsq(A * w[:, None], z * w, rcond=None)
+ except np.linalg.LinAlgError:
+ return None
+ res = z - A @ coef
+ s = 1.4826 * np.median(np.abs(res - np.median(res))) + 1e-3
+ w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
+ return coef
+
+
+@dataclass
+class Corridor:
+ """Осевая линия пути впереди: u_c(d) = c0 + c1·d + c2·d².
+
+ Оценивается по дрейфу центра сечения тоннеля с расстоянием. В прямом
+ тоннеле c1 ≈ c2 ≈ 0; в кривой радиуса R член c2 ≈ 1/(2R). Нужна, чтобы
+ габарит на 150 м впереди не «въезжал» в стену на повороте — иначе вся
+ дальняя зона кривой превращается в сплошное ложное срабатывание.
+ """
+
+ coef: np.ndarray # (3,)
+ d_max_seen: float # дальше этого — экстраполяция
+ n_slices: int
+ radius: float # оценка радиуса кривой, м (inf для прямой)
+
+ def centre(self, d: np.ndarray) -> np.ndarray:
+ """Ось пути на дальности d.
+
+ За пределами наблюдавшейся дальности парабола продолжается **линейно**,
+ по касательной: экстраполировать кривизну туда, где данных не было,
+ значит получить десятки метров ошибки на ровном месте.
+ """
+ d = np.asarray(d, np.float32)
+ c0, c1, c2 = self.coef
+ dm = np.float32(max(self.d_max_seen, 1.0))
+ d_in = np.minimum(d, dm)
+ u = c0 + c1 * d_in + c2 * d_in * d_in
+ slope = c1 + 2.0 * c2 * dm
+ return (u + slope * np.maximum(d - dm, 0.0)).astype(np.float32)
+
+ def sigma(self, d: np.ndarray, base: float = 0.25, rate: float = 0.004) -> np.ndarray:
+ """Неопределённость положения оси: растёт с дальностью и за горизонтом видимости."""
+ d = np.asarray(d, np.float32)
+ extra = np.maximum(d - np.float32(self.d_max_seen), 0.0)
+ return (base + rate * d + 0.02 * extra).astype(np.float32)
+
+
+STRAIGHT = Corridor(np.zeros(3), 0.0, 0, float("inf"))
+
+MIN_TRACK_RADIUS = 300.0 # м, круче на перегонах метрополитена не бывает
+MAX_AXIS_RATE = 0.35 # м за кадр, предел изменения оси на дальности 100 м
+
+
+def fit_corridor(tf: "TrackFrame", *, d_lo: float = 8.0, d_hi: float = 220.0,
+ n_slices: int = 30, h_lo: float = 0.6, h_hi: float = 3.2,
+ min_pts: int = 60, d_ref: float = 22.0,
+ prev: Corridor | None = None, smooth: float = 0.08) -> Corridor:
+ """Оценить осевую линию пути по смещению центра сечения тоннеля.
+
+ Каждый срез по дальности даёт одну оценку центра свода. Веса берутся
+ **равными по срезам**, а не по числу точек: у ближних срезов точек в сотни
+ раз больше, и взвешивание по количеству полностью подавило бы дальние срезы,
+ в которых как раз и содержится кривизна.
+ """
+ edges = np.geomspace(d_lo, d_hi, n_slices + 1)
+ ds, us = [], []
+ for lo, hi in zip(edges[:-1], edges[1:]):
+ m = tf.valid & (tf.d >= lo) & (tf.d < hi) & (tf.h > h_lo) & (tf.h < h_hi)
+ if int(m.sum()) < min_pts:
+ continue
+ uu = tf.u[m]
+ lo_u, hi_u = np.percentile(uu, (3.0, 97.0))
+ if hi_u - lo_u < 1.5: # видна только одна стена — центр не определить
+ continue
+ ds.append(0.5 * (lo + hi))
+ us.append(0.5 * (lo_u + hi_u))
+ if len(ds) < 5:
+ return prev or STRAIGHT
+
+ d = np.asarray(ds, np.float64)
+ u = np.asarray(us, np.float64)
+
+ # положение поезда в сечении: медиана центров ближней зоны
+ ref = d <= d_ref
+ u = u - (np.median(u[ref]) if ref.sum() >= 2 else u[0])
+
+ far = d > 12.0
+ if far.sum() < 4:
+ return prev or STRAIGHT
+ d_f, u_f = d[far], u[far]
+
+ # u ≈ c1·d + c2·d², равные веса по срезам, две итерации робастного отсева
+ A = np.stack([d_f, d_f * d_f], axis=1)
+ w = np.ones_like(u_f)
+ c = np.zeros(2)
+ for _ in range(3):
+ try:
+ c, *_ = np.linalg.lstsq(A * w[:, None], u_f * w, rcond=None)
+ except np.linalg.LinAlgError:
+ return prev or STRAIGHT
+ res = u_f - A @ c
+ s = 1.4826 * np.median(np.abs(res - np.median(res))) + 0.05
+ w = 1.0 / np.sqrt(1.0 + (res / (2.0 * s)) ** 2)
+
+ coef = np.array([0.0, c[0], c[1]])
+ radius = float(abs(1.0 / (2.0 * c[1]))) if abs(c[1]) > 1e-7 else float("inf")
+ # Радиус круче 300 м на перегоне метрополитена не встречается. Такая оценка
+ # означает не кривую, а испорченное сечение: на станции платформа делает свод
+ # резко несимметричным, центр «уезжает», и габарит вместе с ним заезжает
+ # прямо на платформу — источник почти всех ложных тревог у станций.
+ if radius < MIN_TRACK_RADIUS:
+ return prev or STRAIGHT
+
+ out = Corridor(coef, float(d.max()), len(ds), radius)
+ if prev is None or smooth <= 0:
+ return out
+
+ k = smooth
+ blended = k * out.coef + (1 - k) * prev.coef
+ # путь физически не может вильнуть: ограничиваем скорость изменения оси
+ shift_now = blended[1] * 100.0 + blended[2] * 100.0 ** 2
+ shift_prev = prev.coef[1] * 100.0 + prev.coef[2] * 100.0 ** 2
+ excess = abs(shift_now - shift_prev)
+ if excess > MAX_AXIS_RATE:
+ t = MAX_AXIS_RATE / excess
+ blended = prev.coef + (blended - prev.coef) * t
+ r2 = (float(abs(1.0 / (2.0 * blended[2]))) if abs(blended[2]) > 1e-7 else float("inf"))
+ return Corridor(blended, out.d_max_seen, out.n_slices, r2)
+
+
+class TrackFrame:
+ """Кадр в координатах пути: (d, u, h) плюс ожидаемая дальность до пола."""
+
+ __slots__ = ("d", "u", "h", "z", "r", "valid", "inten", "floor_r", "plane",
+ "layout", "img")
+
+ def __init__(self, img: RangeImage, layout: ScanLayout, plane: RailPlane):
+ xyz = img.xyz(layout)
+ self.layout = layout
+ self.img = img
+ self.plane = plane
+ self.u = xyz[..., 0]
+ self.d = -xyz[..., 1]
+ self.z = xyz[..., 2] # в системе сенсора, не над рельсом
+ self.h = plane.height_of(self.d, self.u, self.z)
+ self.r = img.r_near
+ self.valid = img.valid
+ self.inten = img.inten
+ self.floor_r = plane.floor_range(layout)
+
+ def lateral(self, corridor: "Corridor | None" = None) -> np.ndarray:
+ """Смещение точек от осевой линии пути, м.
+
+ Берётся **меньшее по модулю** из двух: отсчёт от прямой оси и от
+ оценённой кривой. Это объединение двух габаритов, а не замена одного
+ другим, и сделано осознанно: оценка оси неизбежно неточна, а система
+ безопасности не имеет права **сужать** зону поиска по неуверенной
+ оценке. Измерено: замена (а не объединение) поднимала пропуски
+ реального объекта с 1 % до 25 %, экономя при этом лишь 2.5 % кадров
+ с ложной тревогой — размен в неверную сторону.
+ """
+ if corridor is None or corridor.n_slices == 0:
+ return self.u
+ curved = self.u - corridor.centre(self.d)
+ return np.where(np.abs(curved) < np.abs(self.u), curved, self.u)
+
+ def in_gauge(self, half_width: float = 1.7, h_lo: float = 0.05,
+ h_hi: float = 2.2, d_min: float = 3.0, d_max: float = 260.0,
+ corridor: "Corridor | None" = None) -> np.ndarray:
+ """Маска лучей, чьи точки лежат внутри габарита приближения."""
+ return (self.valid & (self.d > d_min) & (self.d < d_max)
+ & (np.abs(self.lateral(corridor)) < half_width)
+ & (self.h > h_lo) & (self.h < h_hi))
diff --git a/ros2_ws/src/flyguard/flyguard/gpu.py b/ros2_ws/src/flyguard/flyguard/gpu.py
new file mode 100644
index 0000000..70fdf38
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/gpu.py
@@ -0,0 +1,500 @@
+"""Видеокарта для плотных стадий конвейера: сетчатка, ламина, кластеризация.
+
+Три стадии работают с целым образом 128 × 600 лучей и занимают больше
+половины кадра: раскладка точек по решётке, ламина (центр минус окружение на
+трёх масштабах) и кластеризация лучей с учётом глубины. Они и перенесены.
+Остальное — плоскость пути, ось, оценка движения, треки, решение — остаётся
+на процессоре: там мелкие массивы и ветвистая логика, и копирование на
+видеокарту стоило бы дороже самого расчёта.
+
+Каждая функция повторяет процессорную версию операция в операцию, в тех же
+типах. Раскладка по порядку точек, ламина и кластеризация дают результат,
+совпадающий с процессорным бит в бит (тесты и сверка на записях,
+EXPERIMENTS п. 21):
+
+* сетчатка — только выборки, умножения и корень, всё в float32 без слияния
+ операций;
+* ламина — равномерные окна считаются через накопленные суммы в float64 и
+ округляются в float32 после каждой оси, ровно как `scipy.ndimage.
+ uniform_filter`: сумма чисел float32 такого диапазона в float64 точна, и
+ результат не зависит от порядка сложения;
+* кластеризация — целочисленный граф; компоненты нумеруются по наименьшему
+ лучу, как у `scipy.sparse.csgraph.connected_components`.
+
+Исключение — раскладка по углам точек (кадры с нарушенным порядком, как в
+синтетике организаторов): арктангенс видеокарты и процессора может разойтись
+в последнем знаке, а numpy сортирует ячейки неустойчиво. Расходятся считанные
+лучи на кадр.
+
+Любой сбой видеокарты — нет драйвера, не хватило памяти, ошибка ядра — ловит
+конвейер: до конца работы он считает на процессоре (`device.
+notify_cuda_error`), а кадр досчитывается там же, решение не теряется.
+"""
+from __future__ import annotations
+
+import warnings
+
+import numpy as np
+
+from .lamina import SCALES, LaminaOutput
+from .retina import DEG, ORDER_TOL_DEG, RangeImage, ScanLayout
+
+
+class GpuStages:
+ """Плотные стадии на видеокарте; держит таблицы решётки и образ кадра."""
+
+ def __init__(self, device: str = "cuda"):
+ import torch
+ self.torch = torch
+ self.dev = torch.device(device)
+ torch.zeros(1, device=self.dev) # поднять контекст сразу, а не на первом кадре
+ self._tab_key = None
+ self._tab: dict = {}
+ self._win: dict = {}
+ self._off: dict = {}
+ self._box: dict = {}
+ self._graphs = True # графы CUDA для ламины; при отказе — по одной операции
+ self._lam_key = None
+ # образ текущего кадра: ламина и кластеризация берут его отсюда, не копируя заново
+ self.r = None
+ self.valid = None
+ self._r_host = None
+
+ def warmup(self, shape: tuple[int, int] = (128, 600)) -> None:
+ """Прогнать все три стадии на выдуманном кадре.
+
+ Первый запуск каждого ядра CUDA подгружает его код: без прогрева первый
+ кадр записи шёл 1–1.7 с, очередь подписки переполнялась, и терялись
+ первые 6–8 кадров (замерено в контейнере). Раскладка прогревается на
+ маленькой решётке обоими путями — по порядку точек и по углам.
+ """
+ from .cdr import PointCloud2
+ t = self.torch
+ rng = np.random.default_rng(0)
+ n, w, e = 8, 64, 2
+ lay = ScanLayout(np.linspace(10.0, -20.0, n), -0.1, 3.2, np.zeros(n, np.int64),
+ np.zeros(n), w, e)
+ xyz = lay.dirs.transpose(1, 0, 2)[:, None, :, :] * np.float32(20.0) # столбец·эхо·кольцо
+ xyz = np.broadcast_to(xyz, (w, e, n, 3)).reshape(-1, 3)
+ pts = np.zeros(xyz.shape[0], dtype=[("x", " dict:
+ key = (id(L), L.n_points, L.n_az)
+ if key != self._tab_key:
+ t, d = self.torch, self.dev
+
+ def up(a, dtype):
+ return t.as_tensor(np.ascontiguousarray(a), dtype=dtype, device=d)
+
+ self._tab = {
+ "sin_el": up(L._sin_el, t.float32),
+ "el_asc": up(L._el_asc, t.float64),
+ "el_order": up(L._el_order, t.int64),
+ "el_deg": up(L.el_deg, t.float64),
+ "el_step": up(L.el_step_deg, t.float64),
+ "resid": up(L.az_resid_deg, t.float64),
+ }
+ self._win = {}
+ self._tab_key = key
+ return self._tab
+
+ def _window(self, L: ScanLayout, start: int, stop: int) -> tuple:
+ """Сырые столбцы, нужные сектору, и карта выпрямления внутри них."""
+ key = (start, stop)
+ if key not in self._win:
+ w = L.n_az
+ lo = start + int(L.col_shift.min())
+ hi = stop + int(L.col_shift.max())
+ g = L.gather[:, start:stop]
+ ok = L.gather_ok[:, start:stop]
+ if L.wrap:
+ raw = np.arange(lo, hi) % w
+ g = (g - lo) % w
+ else:
+ lo, hi = max(lo, 0), min(hi, w)
+ raw = np.arange(lo, hi)
+ g = g - lo
+ ok = ok & (g >= 0) & (g < (hi - lo))
+ g = np.clip(g, 0, hi - lo - 1)
+ # столбцы окна идут подряд, кроме стыка кругового скана
+ cut = np.flatnonzero(np.diff(raw) != 1) + 1
+ runs = [(int(a[0]), int(a[-1]) + 1) for a in np.split(raw, cut) if a.size]
+ t = self.torch
+ self._win[key] = (runs, t.as_tensor(g, dtype=t.int64, device=self.dev),
+ t.as_tensor(ok, device=self.dev))
+ return self._win[key]
+
+ # ------------------------------------------------------------------ загрузка точек
+
+ def _upload(self, pts: np.ndarray, rows: list[tuple[int, int]]):
+ """Строки структурированного массива → байты на видеокарте, (N, шаг точки)."""
+ t = self.torch
+ step = pts.dtype.itemsize
+ parts = []
+ for a, b in rows:
+ raw = np.ascontiguousarray(pts[a:b]).view(np.uint8)
+ with warnings.catch_warnings():
+ # буфер сообщения только для чтения, а тензор мы не пишем
+ warnings.simplefilter("ignore", UserWarning)
+ parts.append(t.from_numpy(raw).to(self.dev, non_blocking=False))
+ buf = parts[0] if len(parts) == 1 else t.cat(parts)
+ return buf.view(-1, step)
+
+ def _field(self, buf, pts: np.ndarray, name: str):
+ dt, off = pts.dtype.fields[name][:2]
+ if dt.kind != "f" or dt.itemsize != 4 or dt.byteorder == ">":
+ raise TypeError(f"поле {name}: ожидался float32, а не {dt}")
+ return buf[:, off:off + 4].contiguous().view(self.torch.float32).reshape(-1)
+
+ # ------------------------------------------------------------------ сетчатка
+
+ def project(self, L: ScanLayout, pc, cols: slice | None) -> RangeImage:
+ """То же, что `ScanLayout.project`, на видеокарте."""
+ w = L.n_az
+ start = 0 if cols is None else (cols.start or 0)
+ stop = w if cols is None else (cols.stop if cols.stop is not None else w)
+ self._tables(L)
+ img = None
+ if L.indexed and pc.n_points == L.n_points:
+ img = self._project_indexed(L, pc, start, stop)
+ if img is None:
+ L.n_geometric += 1
+ img = self._project_geometric(L, pc, start, stop)
+ self._r_host = img.r_near
+ return img
+
+ def _project_indexed(self, L: ScanLayout, pc, start: int, stop: int):
+ t = self.torch
+ tab = self._tab
+ n, e = L.n_rings, L.n_echo
+ runs, g, ok = self._window(L, start, stop)
+ pts = pc.points
+ per_col = e * n
+ buf = self._upload(pts, [(a * per_col, b * per_col) for a, b in runs])
+ ncol = sum(b - a for a, b in runs)
+
+ def cube(name: str):
+ return self._field(buf, pts, name).reshape(ncol, e, n).permute(2, 0, 1)
+
+ x, y, z = cube("x"), cube("y"), cube("z")
+ good = (x != 0) | (y != 0) | (z != 0)
+ r = t.sqrt(x * x + y * y + z * z)
+ good &= t.isfinite(r)
+ r = t.where(good, r, t.zeros((), dtype=r.dtype, device=self.dev))
+
+ dev = t.abs(z - r * tab["sin_el"][:, None, None])
+ tol = r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)
+ if bool(t.any(dev > tol)):
+ return None
+
+ if e == 1:
+ r_near = r[..., 0]
+ r_far = r[..., 0]
+ it = cube("intensity")[..., 0]
+ valid = good[..., 0]
+ else:
+ inten = cube("intensity")
+ inf = t.full((), float("inf"), dtype=r.dtype, device=self.dev)
+ near_i = t.argmin(t.where(good, r, inf), dim=-1, keepdim=True)
+ far_i = t.argmax(r, dim=-1, keepdim=True)
+ r_near = t.gather(r, -1, near_i)[..., 0]
+ r_far = t.gather(r, -1, far_i)[..., 0]
+ it = t.gather(inten, -1, near_i)[..., 0]
+ valid = good.any(dim=-1)
+
+ r_near = t.gather(r_near, 1, g)
+ r_far = t.gather(r_far, 1, g)
+ it = t.gather(it, 1, g)
+ valid = t.gather(valid, 1, g) & ok
+ zero = t.zeros((), dtype=r_near.dtype, device=self.dev)
+ r_near = t.where(valid, r_near, zero)
+ r_far = t.where(valid, r_far, zero)
+ return self._finish(pc.stamp, r_near, r_far, it, valid)
+
+ def _project_geometric(self, L: ScanLayout, pc, start: int, stop: int):
+ t = self.torch
+ tab = self._tab
+ n, wid = L.n_rings, stop - start
+ pts = pc.points
+ buf = self._upload(pts, [(0, pts.shape[0])])
+ x, y, z = (self._field(buf, pts, k) for k in ("x", "y", "z"))
+ good = (((x != 0) | (y != 0) | (z != 0)) & t.isfinite(x) & t.isfinite(y)
+ & t.isfinite(z))
+ idx = t.nonzero(good, as_tuple=True)[0]
+ x, y, z = x[idx], y[idx], z[idx]
+
+ step = L.az_step_deg
+ az = t.rad2deg(t.atan2(x, -y))
+ jf = (az - np.float32(L.az0_deg)) / np.float32(step)
+ if L.wrap:
+ jf = t.remainder(jf, L.n_az)
+ margin = float(np.abs(L.az_resid_deg).max()) / abs(step) + 1.0
+ sel = t.nonzero((jf > start - margin) & (jf < stop - 1 + margin), as_tuple=True)[0]
+ idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]
+
+ r = t.sqrt(x * x + y * y + z * z)
+ el = t.rad2deg(t.asin(t.clamp(z / t.clamp_min(r, np.float32(1e-6)), -1.0, 1.0)))
+ el64 = el.double()
+ asc = tab["el_asc"]
+ k = t.clamp(t.searchsorted(asc, el64), 1, n - 1)
+ k = k - ((el64 - asc[k - 1]) < (asc[k] - el64)).long()
+ h = tab["el_order"][k]
+ ok = t.abs(el64 - tab["el_deg"][h]) <= t.clamp_min(tab["el_step"][h], 0.2)
+ j = t.round(((az.double() - tab["resid"][h]) - L.az0_deg) / step).long()
+ if L.wrap:
+ j = t.remainder(j, L.n_az)
+ ok &= (j >= start) & (j < stop)
+ sel = t.nonzero(ok, as_tuple=True)[0]
+
+ r_near = t.zeros(n * wid, dtype=t.float32, device=self.dev)
+ r_far = t.zeros_like(r_near)
+ it = t.zeros_like(r_near)
+ valid = t.zeros(n * wid, dtype=t.bool, device=self.dev)
+ if sel.numel():
+ cell = h[sel] * wid + (j[sel] - start)
+ rr = r[sel]
+ mm = t.clamp_max(rr * 1000.0, float((1 << 20) - 1)).long()
+ order = t.argsort(cell * (1 << 20) + mm, stable=True)
+ cs, rs = cell[order], rr[order]
+ new = t.ones(cs.numel(), dtype=t.bool, device=self.dev)
+ new[1:] = cs[1:] != cs[:-1]
+ first = t.nonzero(new, as_tuple=True)[0]
+ last = t.cat([first[1:] - 1, first.new_tensor([cs.numel() - 1])])
+ r_near[cs[first]] = rs[first]
+ r_far[cs[last]] = rs[last]
+ valid[cs[first]] = True
+ if "intensity" in pts.dtype.names:
+ src = self._field(buf, pts, "intensity")[idx[sel]][order]
+ it[cs[first]] = src[first]
+ return self._finish(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
+ it.reshape(n, wid), valid.reshape(n, wid))
+
+ def _finish(self, stamp: float, r_near, r_far, it, valid) -> RangeImage:
+ self.r, self.valid = r_near, valid
+ host = [a.cpu().numpy() for a in (r_near, r_far, it, valid)]
+ return RangeImage(stamp, *host)
+
+ # ------------------------------------------------------------------ ламина
+
+ def _box_axis(self, X, sizes: list[int], dim: int):
+ """Равномерное окно по одной оси для пачки размеров сразу.
+
+ `X` — (K, B, H, W) float32, k-й срез фильтруется окном `sizes[k]`
+ вдоль `dim` (2 или 3) с продолжением краевым значением. Суммы окон
+ берутся из накопленной суммы в float64: все частичные суммы чисел
+ float32 такого диапазона в float64 точны, поэтому сумма окна точна при
+ любом порядке сложения. Деление на размер и округление в float32 —
+ как в `scipy.ndimage.uniform_filter1d`.
+ """
+ t = self.torch
+ Xd = X.double()
+ n = Xd.shape[dim]
+ lo, hi, n_lo, n_hi, size = self._box_tables(tuple(sizes), n, dim)
+ cs = t.cumsum(Xd, dim=dim)
+ cs = t.cat([t.zeros_like(cs.narrow(dim, 0, 1)), cs], dim=dim)
+
+ def along(a):
+ return a.expand(*Xd.shape[:dim], n, *Xd.shape[dim + 1:])
+
+ s = (t.gather(cs, dim, along(hi)) - t.gather(cs, dim, along(lo))
+ + along(n_lo) * Xd.narrow(dim, 0, 1) + along(n_hi) * Xd.narrow(dim, n - 1, 1))
+ return (s / size).float()
+
+ def _box_tables(self, sizes: tuple, n: int, dim: int):
+ """Границы окон по оси: считаются один раз (и до записи графа CUDA)."""
+ key = (sizes, n, dim)
+ if key not in self._box:
+ t = self.torch
+ p = t.as_tensor([s // 2 for s in sizes], dtype=t.int64, device=self.dev)
+ i = t.arange(n, device=self.dev)
+ shape = [len(sizes), 1, 1, 1]
+ shape[dim] = n
+ lo = (i[None, :] - p[:, None]).clamp(min=0).reshape(shape)
+ hi = ((i[None, :] + p[:, None]).clamp(max=n - 1) + 1).reshape(shape)
+ n_lo = (p[:, None] - i[None, :]).clamp(min=0).double().reshape(shape)
+ n_hi = (i[None, :] + p[:, None] - (n - 1)).clamp(min=0).double().reshape(shape)
+ size = t.as_tensor(sizes, dtype=t.float64, device=self.dev).reshape(-1, 1, 1, 1)
+ self._box[key] = (lo, hi, n_lo, n_hi, size)
+ return self._box[key]
+
+ def _boxes(self, X, sizes: list[tuple[int, int]]):
+ """`scipy.ndimage.uniform_filter(x, size, mode="nearest")` для пачки.
+
+ `X` — (B, H, W) float32; выход (len(sizes), B, H, W): по осям по
+ очереди, с округлением в float32 после каждой, как в scipy.
+ """
+ K = len(sizes)
+ Y = self._box_axis(X[None].expand(K, *X.shape), [s[0] for s in sizes], 2)
+ return self._box_axis(Y, [s[1] for s in sizes], 3)
+
+ def lamina(self, r_max: float = 300.0) -> LaminaOutput:
+ """То же, что `lamina._process_cpu`, по образу текущего кадра.
+
+ Сама видеокарта считает ламину за доли миллисекунды, а запуск сотни
+ мелких операций из Python стоит в несколько раз дороже. Поэтому ламина
+ записывается в граф CUDA один раз на размер образа и дальше
+ запускается одной командой. Не поддерживает граф драйвер — считаем
+ теми же операциями по одной.
+ """
+ t = self.torch
+ key = (tuple(self.r.shape), float(r_max))
+ out = None
+ if self._graphs:
+ try:
+ if self._lam_key != key:
+ self._capture_lamina(key, r_max)
+ self._lam_in[0].copy_(self.r)
+ self._lam_in[1].copy_(self.valid)
+ self._lam_graph.replay()
+ out = self._lam_out
+ except Exception:
+ self._graphs = False
+ self._lam_key = None
+ if out is None:
+ out = self._lamina_core(self.r, self.valid, r_max)
+ f = out[0].cpu().numpy()
+ return LaminaOutput(disp=f[0], on=f[1], off=f[2], on_scale=out[1].cpu().numpy(),
+ surround=f[3], hole=f[4])
+
+ def _capture_lamina(self, key: tuple, r_max: float) -> None:
+ t = self.torch
+ self._lam_in = (t.empty_like(self.r), t.empty_like(self.valid))
+ self._lam_in[0].copy_(self.r)
+ self._lam_in[1].copy_(self.valid)
+ side = t.cuda.Stream(self.dev)
+ side.wait_stream(t.cuda.current_stream(self.dev))
+ with t.cuda.stream(side):
+ for _ in range(2): # прогрев: таблицы окон и кэш памяти
+ self._lamina_core(*self._lam_in, r_max)
+ t.cuda.current_stream(self.dev).wait_stream(side)
+ g = t.cuda.CUDAGraph()
+ with t.cuda.graph(g):
+ self._lam_out = self._lamina_core(*self._lam_in, r_max)
+ self._lam_graph, self._lam_key = g, key
+
+ def _lamina_core(self, r, valid, r_max: float):
+ """Ламина целиком на видеокарте: (disp, on, off, surround, hole) и on_scale."""
+ t = self.torch
+ v = valid.float()
+ zero = t.zeros((), dtype=t.float32, device=self.dev)
+ disp = t.where(valid & (r > 0.05), 1.0 / r, zero) * v
+
+ # все окна кадра одной пачкой: на каждом масштабе центр и окружение,
+ # для диспаритета и для маски эха, плюс окно «дыры»
+ sizes = []
+ for r_in, r_out in SCALES:
+ sizes += [(2 * r_in + 1, 2 * r_in + 1), (2 * r_out + 1, 4 * r_out + 1)]
+ sizes.append((5, 15))
+ box = self._boxes(t.stack([disp, v]), sizes)
+
+ on = t.zeros_like(disp)
+ off = t.zeros_like(disp)
+ on_scale = t.zeros(disp.shape, dtype=t.int8, device=self.dev)
+ surround_mid = None
+ for k, (r_in, r_out) in enumerate(SCALES):
+ s_in, s_out = sizes[2 * k], sizes[2 * k + 1]
+ n_in = s_in[0] * s_in[1]
+ n_out = s_out[0] * s_out[1]
+ num = box[2 * k + 1, 0] * n_out - box[2 * k, 0] * n_in
+ den = box[2 * k + 1, 1] * n_out - box[2 * k, 1] * n_in
+ sur = t.where(den > 0.5, num / den, zero)
+ enough = den > 8.0
+ c = t.where(enough, disp - sur, zero)
+ pos = t.clamp_min(c, 0.0) * v
+ neg = t.clamp_min(-(disp - sur), 0.0) * enough
+ better = pos > on
+ on = t.where(better, pos, on)
+ on_scale = t.where(better, t.full((), k, dtype=t.int8, device=self.dev), on_scale)
+ off = t.maximum(off, neg)
+ if k == 1:
+ surround_mid = sur
+ hole = 1.0 - box[-1, 1]
+ on = t.clamp(on, 0.0, 1.0 / max(r_max, 1.0) * 1e4)
+ return t.stack([disp, on, off, surround_mid, hole]), on_scale
+
+ # ------------------------------------------------------------------ кластеризация
+
+ def _offsets(self, col_reach: int, row_reach: int):
+ key = (col_reach, row_reach)
+ if key not in self._off:
+ pairs = [(dr, dc) for dr in range(0, row_reach + 1)
+ for dc in range(-col_reach, col_reach + 1) if not (dr == 0 and dc <= 0)]
+ p = self.torch.tensor(pairs, dtype=self.torch.int64, device=self.dev)
+ self._off[key] = (p[:, 0:1], p[:, 1:2])
+ return self._off[key]
+
+ def cluster_by_depth(self, mask: np.ndarray, r: np.ndarray, *,
+ rel_tol: float = 0.06, abs_tol: float = 0.35,
+ col_reach: int = 3, row_reach: int = 2):
+ """То же, что `lobula.cluster_by_depth`, с теми же номерами компонент.
+
+ Рёбра строятся сразу для всех соседей, компоненты — подвешиванием к
+ меньшему номеру со сжатием путей: у каждой компоненты корнем остаётся
+ её наименьший луч, и нумерация по корням совпадает со scipy.
+ """
+ t = self.torch
+ h, w = mask.shape
+ if r is self._r_host and self.r is not None:
+ rt = self.r.reshape(-1)
+ else:
+ rt = t.as_tensor(np.ascontiguousarray(r), dtype=t.float32,
+ device=self.dev).reshape(-1)
+ m = t.as_tensor(np.ascontiguousarray(mask), device=self.dev).reshape(-1)
+ idx = t.nonzero(m, as_tuple=True)[0]
+ n = idx.numel()
+ if n == 0:
+ return np.zeros(mask.shape, np.int32), 0
+ lut = t.full((h * w,), -1, dtype=t.int64, device=self.dev)
+ lut[idx] = t.arange(n, device=self.dev)
+ rows = t.div(idx, w, rounding_mode="floor")
+ cols = idx - rows * w
+ ri = rt[idx]
+ dr, dc = self._offsets(col_reach, row_reach)
+ nr = rows[None, :] + dr
+ nc = cols[None, :] + dc
+ ok = (nr < h) & (nc >= 0) & (nc < w)
+ nb = lut[t.where(ok, nr * w + nc, t.zeros_like(nr))]
+ ok &= nb >= 0
+ rb = ri[nb.clamp(min=0)]
+ ra = ri[None, :].expand_as(rb)
+ ok &= t.abs(ra - rb) <= (abs_tol + rel_tol * t.minimum(ra, rb))
+ s = t.arange(n, device=self.dev)[None, :].expand_as(nb)[ok]
+ d = nb[ok]
+
+ parent = t.arange(n, device=self.dev)
+ while True:
+ for _ in range(4):
+ ps, pd = parent[s], parent[d]
+ parent.scatter_reduce_(0, t.maximum(ps, pd), t.minimum(ps, pd), reduce="amin")
+ parent = parent[parent]
+ parent = parent[parent]
+ if not bool((parent[s] != parent[d]).any()):
+ break
+ while True:
+ nxt = parent[parent]
+ if bool(t.equal(nxt, parent)):
+ break
+ parent = nxt
+ roots, comp = t.unique(parent, return_inverse=True)
+ labels = t.zeros(h * w, dtype=t.int32, device=self.dev)
+ labels[idx] = (comp + 1).to(t.int32)
+ return labels.reshape(h, w).cpu().numpy(), int(roots.numel())
diff --git a/ros2_ws/src/flyguard/flyguard/lamina.py b/ros2_ws/src/flyguard/flyguard/lamina.py
new file mode 100644
index 0000000..df4beda
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/lamina.py
@@ -0,0 +1,216 @@
+"""LAMINA — локальный контраст, разделение ON/OFF.
+
+Первый нейропиль за фоторецепторами. Клетки L1 и L2 получают один и тот же вход
+от R1–R6 и расходятся на два канала: L1 → ON (стало ярче), L2 → OFF (стало
+темнее). Оба канала предварительно проходят **латеральное торможение** от
+амакриновых клеток и Dm9 — классическое «центр минус окружение», которое
+подавляет ровный фон и оставляет только локальные отклонения.
+
+Перенос на лидар:
+
+* «Яркость» — это **диспаритет** δ = 1/R, а не сама дальность. Так правильно
+ по двум причинам: угловой размер предмета пропорционален 1/R, и шум лидара
+ в диспаритете почти однороден, тогда как в дальности растёт квадратично.
+* **ON** = объект ближе своего окружения — выступ, то есть препятствие.
+* **OFF** = дальше окружения или эха нет вовсе — провал, то есть окклюзионная
+ тень **за** препятствием. Тень часто во много раз крупнее самого предмета,
+ и именно она даёт шанс увидеть мелкий объект на большой дальности.
+* Размер предмета в лучах меняется с дальностью на два порядка, поэтому
+ окружение берётся **на нескольких масштабах** сразу — как у колонковых
+ нейронов лобулы с разными размерами рецептивных полей, сходящихся на один
+ нисходящий нейрон.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.ndimage import uniform_filter
+
+# (радиус центра, радиус окружения) в лучах: кольца × столбцы
+SCALES: tuple[tuple[int, int], ...] = ((1, 6), (3, 14), (7, 30))
+
+
+@dataclass
+class LaminaOutput:
+ """Каналы ламины для одного кадра."""
+
+ disp: np.ndarray # (H, W) диспаритет 1/R, 0 там, где эха нет
+ on: np.ndarray # (H, W) ON-контраст, максимум по масштабам, 1/м
+ off: np.ndarray # (H, W) OFF-контраст, 1/м
+ on_scale: np.ndarray # (H, W) int8 — на каком масштабе отклик максимален
+ surround: np.ndarray # (H, W) диспаритет окружения на среднем масштабе
+ hole: np.ndarray # (H, W) доля «нет эха» в окрестности
+
+
+def _masked_mean(v: np.ndarray, m: np.ndarray, size: tuple[int, int]) -> np.ndarray:
+ """Среднее по прямоугольному окну только по валидным отсчётам."""
+ num = uniform_filter(v, size=size, mode="nearest")
+ den = uniform_filter(m, size=size, mode="nearest")
+ return num, den
+
+
+def _annulus_mean(v: np.ndarray, m: np.ndarray, r_in: int, r_out: int):
+ """Среднее по кольцу: большое окно минус вырезанный центр.
+
+ Реализовано через два равномерных фильтра (каждый разделим и работает за
+ O(N)), поэтому стоимость не зависит от размера окна.
+ """
+ s_in = (2 * r_in + 1, 2 * r_in + 1)
+ s_out = (2 * r_out + 1, 4 * r_out + 1) # шире по азимуту: решётка анизотропна
+ n_in = s_in[0] * s_in[1]
+ n_out = s_out[0] * s_out[1]
+
+ num_i, den_i = _masked_mean(v, m, s_in)
+ num_o, den_o = _masked_mean(v, m, s_out)
+
+ num = num_o * n_out - num_i * n_in
+ den = den_o * n_out - den_i * n_in
+ out = np.divide(num, den, out=np.zeros_like(num), where=den > 0.5)
+ return out, den
+
+
+def _process_cpu(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0) -> LaminaOutput:
+ """CPU-реализация через SciPy uniform_filter."""
+ v = valid.astype(np.float32)
+ disp = np.zeros_like(r, dtype=np.float32)
+ np.divide(1.0, r, out=disp, where=valid & (r > 0.05))
+ disp *= v
+
+ on = np.zeros_like(disp)
+ off = np.zeros_like(disp)
+ on_scale = np.zeros(disp.shape, np.int8)
+ surround_mid = None
+
+ for k, (r_in, r_out) in enumerate(SCALES):
+ sur, cnt = _annulus_mean(disp, v, r_in, r_out)
+ enough = cnt > 8.0
+ c = np.where(enough, disp - sur, 0.0)
+ pos = np.maximum(c, 0.0) * v # ближе окружения
+ # провал считается и там, где эха нет: 1/∞ = 0 — это тоже сигнал
+ neg = np.maximum(-(disp - sur), 0.0) * enough
+ better = pos > on
+ on = np.where(better, pos, on)
+ on_scale = np.where(better, np.int8(k), on_scale)
+ off = np.maximum(off, neg)
+ if k == 1:
+ surround_mid = sur
+
+ # доля лучей без эха в окрестности — мера «дыры» в поверхности
+ hole = 1.0 - uniform_filter(v, size=(5, 15), mode="nearest")
+
+ # диспаритет физически ограничен снизу дальностью прибора
+ np.clip(on, 0.0, 1.0 / max(r_max, 1.0) * 1e4, out=on)
+ return LaminaOutput(disp=disp, on=on, off=off, on_scale=on_scale,
+ surround=surround_mid if surround_mid is not None else np.zeros_like(disp),
+ hole=hole.astype(np.float32))
+
+
+def _process_gpu(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0, device: str = "cuda") -> LaminaOutput:
+ """Ускоренная GPU-реализация 2D-фильтрации DoG через PyTorch CUDA тензоры.
+
+ На NVIDIA RTX 4070 Ti Super сокращает время расчета кадра с 8 мс до 0.25 мс.
+ """
+ import torch
+ import torch.nn.functional as F
+
+ with torch.no_grad():
+ dev = torch.device(device)
+ r_t = torch.as_tensor(r, dtype=torch.float32, device=dev)
+ v_t = torch.as_tensor(valid, dtype=torch.float32, device=dev)
+
+ mask_valid = (v_t > 0.5) & (r_t > 0.05)
+ disp_t = torch.where(mask_valid, 1.0 / r_t, torch.zeros_like(r_t)) * v_t
+
+ disp_4d = disp_t.unsqueeze(0).unsqueeze(0) # (1, 1, H, W)
+ v_4d = v_t.unsqueeze(0).unsqueeze(0)
+
+ on_t = torch.zeros_like(disp_t)
+ off_t = torch.zeros_like(disp_t)
+ on_scale_t = torch.zeros_like(disp_t, dtype=torch.int8)
+ surround_mid_t = None
+
+ for k, (r_in, r_out) in enumerate(SCALES):
+ pad_i = (r_in, r_in, r_in, r_in)
+ pad_o = (2 * r_out, 2 * r_out, r_out, r_out)
+
+ k_in = (2 * r_in + 1, 2 * r_in + 1)
+ k_out = (2 * r_out + 1, 4 * r_out + 1)
+
+ disp_pad_i = F.pad(disp_4d, pad_i, mode='replicate')
+ disp_pad_o = F.pad(disp_4d, pad_o, mode='replicate')
+ v_pad_i = F.pad(v_4d, pad_i, mode='replicate')
+ v_pad_o = F.pad(v_4d, pad_o, mode='replicate')
+
+ n_in = float(k_in[0] * k_in[1])
+ n_out = float(k_out[0] * k_out[1])
+
+ sum_disp_i = F.avg_pool2d(disp_pad_i, k_in, stride=1) * n_in
+ sum_disp_o = F.avg_pool2d(disp_pad_o, k_out, stride=1) * n_out
+ sum_v_i = F.avg_pool2d(v_pad_i, k_in, stride=1) * n_in
+ sum_v_o = F.avg_pool2d(v_pad_o, k_out, stride=1) * n_out
+
+ num_t = (sum_disp_o - sum_disp_i).squeeze(0).squeeze(0)
+ den_t = (sum_v_o - sum_v_i).squeeze(0).squeeze(0)
+
+ sur_t = torch.where(den_t > 0.5, num_t / den_t, torch.zeros_like(num_t))
+ enough_t = den_t > 8.0
+
+ c_t = torch.where(enough_t, disp_t - sur_t, torch.zeros_like(disp_t))
+ pos_t = torch.clamp_min(c_t, 0.0) * v_t
+ neg_t = torch.clamp_min(-(disp_t - sur_t), 0.0) * enough_t.float()
+
+ better_t = pos_t > on_t
+ on_t = torch.where(better_t, pos_t, on_t)
+ on_scale_t = torch.where(better_t, torch.tensor(k, dtype=torch.int8, device=dev), on_scale_t)
+ off_t = torch.maximum(off_t, neg_t)
+
+ if k == 1:
+ surround_mid_t = sur_t
+
+ pad_hole = (7, 7, 2, 2)
+ v_pad_h = F.pad(v_4d, pad_hole, mode='replicate')
+ hole_mean = F.avg_pool2d(v_pad_h, (5, 15), stride=1).squeeze(0).squeeze(0)
+ hole_t = 1.0 - hole_mean
+
+ on_t = torch.clamp(on_t, 0.0, 1.0 / max(r_max, 1.0) * 1e4)
+
+ return LaminaOutput(
+ disp=disp_t.cpu().numpy(),
+ on=on_t.cpu().numpy(),
+ off=off_t.cpu().numpy(),
+ on_scale=on_scale_t.cpu().numpy(),
+ surround=surround_mid_t.cpu().numpy() if surround_mid_t is not None else np.zeros_like(r, dtype=np.float32),
+ hole=hole_t.cpu().numpy().astype(np.float32)
+ )
+
+
+def process(r: np.ndarray, valid: np.ndarray, *, r_max: float = 300.0,
+ device: str | None = None) -> LaminaOutput:
+ """Посчитать ON/OFF-каналы ламины по дальностному образу (автовыбор GPU / CPU)."""
+ target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
+ if target_dev == "auto":
+ from .device import get_device
+ target_dev = get_device("auto")
+
+ if target_dev.startswith("cuda"):
+ try:
+ return _process_gpu(r, valid, r_max=r_max, device=target_dev)
+ except Exception as e:
+ from .device import notify_cuda_error
+ notify_cuda_error(e)
+ return _process_cpu(r, valid, r_max=r_max)
+
+ return _process_cpu(r, valid, r_max=r_max)
+
+
+def contrast_to_depth_gap(on: np.ndarray, r: np.ndarray) -> np.ndarray:
+ """Перевести ON-контраст диспаритета в «насколько ближе окружения», м.
+
+ δ − δ_sur = 1/R − 1/R_sur ⇒ R_sur − R = on · R · R_sur. Для оценки берётся
+ R_sur = R/(1 − on·R), что даёт разрыв по глубине в метрах.
+ """
+ x = np.clip(on * r, 0.0, 0.999)
+ with np.errstate(divide="ignore", invalid="ignore"):
+ gap = r * x / (1.0 - x)
+ return np.nan_to_num(gap, nan=0.0, posinf=1e4).astype(np.float32)
diff --git a/ros2_ws/src/flyguard/flyguard/lobula.py b/ros2_ws/src/flyguard/flyguard/lobula.py
new file mode 100644
index 0000000..39d2522
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/lobula.py
@@ -0,0 +1,533 @@
+"""LOBULA — колонковые детекторы объектов (аналог LC11) и сборка кандидатов.
+
+В лобуле мухи сидят колонковые нейроны LC: каждый смотрит в свой участок поля
+зрения и реагирует на **признак**, а не на картинку целиком. LC11 — детектор
+мелкого объекта: он возбуждается компактным пятном, выделяющимся из фона, и
+подавляется широкопольным движением, то есть собственным перемещением мухи.
+Выход LC-популяции — не изображение, а короткий список «где что-то есть».
+
+Здесь тем же занимается `find_candidates`: из ретинотопических масок улик
+собираются связные пятна, для каждого считаются геометрические признаки, и
+дальше по конвейеру идут уже десятки кандидатов вместо сотен тысяч лучей.
+
+Решение о том, препятствие это или штатная конструкция тоннеля, здесь НЕ
+принимается — этим занимаются грибовидное тело (знакомость) и центральный
+комплекс (накопление улик по кадрам).
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+from scipy import ndimage
+
+from .geometry import Corridor, TrackFrame
+from .lamina import LaminaOutput, contrast_to_depth_gap
+
+_NEIGHBOURS = np.ones((3, 3), bool)
+
+
+@dataclass
+class Candidate:
+ """Кандидат в препятствия — компактная группа лучей."""
+
+ d: float # расстояние вперёд, м
+ u: float # смещение от оси пути, м
+ h: float # высота над головкой рельса, м
+ d_min: float # ближняя граница, м
+ h_min: float # нижняя точка над рельсом, м — есть ли опора снизу
+ width: float # ширина, м
+ height: float # высота, м
+ depth: float # протяжённость вдоль пути, м
+ containment: float # доля лучей пятна, оставшихся внутри габарита, 0…1
+ n_rays: int # число лучей
+ n_rings: int
+ n_cols: int
+ gap: float # насколько ближе фона, м
+ on: float # ON-контраст, 1/м
+ floor_deficit: float # насколько раньше оборвался луч до полотна, м
+ shadow: float # доля тени (нет эха) сразу за объектом
+ inten: float # медианная интенсивность
+ az_deg: float
+ el_deg: float
+ bbox: tuple[int, int, int, int]
+ score: float = 0.0
+ novelty: float = 1.0
+ extra: dict = field(default_factory=dict)
+
+ @property
+ def area(self) -> float:
+ return self.width * self.height
+
+
+def cluster_by_depth(mask: np.ndarray, r: np.ndarray, *,
+ rel_tol: float = 0.06, abs_tol: float = 0.35,
+ col_reach: int = 3, row_reach: int = 2):
+ """Связные компоненты с учётом разрыва по глубине.
+
+ Обычная связность по соседству склеивает предмет со стеной, которая
+ оказалась в том же месте изображения, но на сто метров дальше. Поэтому два
+ соседних луча объединяются только если их дальности близки — допуск растёт
+ с расстоянием, потому что и разрешение, и шум растут так же.
+
+ Соседство берётся с запасом по азимуту (`col_reach`), чтобы пропуски эха
+ внутри предмета не дробили его на части.
+
+ Возвращает (labels, n_labels); метка 0 — фон.
+ """
+ idx = np.flatnonzero(mask.ravel())
+ if idx.size == 0:
+ return np.zeros(mask.shape, np.int32), 0
+
+ h, w = mask.shape
+ lut = np.full(mask.size, -1, np.int64)
+ lut[idx] = np.arange(idx.size)
+ ri = r.ravel()[idx]
+ rows, cols = np.divmod(idx, w)
+
+ src, dst = [], []
+ for dr in range(0, row_reach + 1):
+ for dc in range(-col_reach, col_reach + 1):
+ if dr == 0 and dc <= 0:
+ continue
+ nr, nc = rows + dr, cols + dc
+ ok = (nr < h) & (nc >= 0) & (nc < w)
+ nb = lut[np.where(ok, nr * w + nc, 0)]
+ ok &= nb >= 0
+ if not ok.any():
+ continue
+ a = np.flatnonzero(ok)
+ b = nb[ok]
+ close = np.abs(ri[a] - ri[b]) <= (abs_tol + rel_tol * np.minimum(ri[a], ri[b]))
+ if close.any():
+ src.append(a[close])
+ dst.append(b[close])
+
+ n_nodes = idx.size
+ if src:
+ from scipy.sparse import coo_matrix
+ from scipy.sparse.csgraph import connected_components
+ s = np.concatenate(src)
+ t = np.concatenate(dst)
+ g = coo_matrix((np.ones(s.size, np.int8), (s, t)), shape=(n_nodes, n_nodes))
+ n_comp, comp = connected_components(g, directed=False)
+ else:
+ n_comp = n_nodes
+ comp = np.arange(n_nodes)
+
+ labels = np.zeros(mask.size, np.int32)
+ labels[idx] = comp + 1
+ return labels.reshape(mask.shape), n_comp
+
+
+def split_by_figure(labels: np.ndarray, n: int, r: np.ndarray,
+ figure: np.ndarray, *, max_depth: float, thr: float,
+ col_reach: int = 3, row_reach: int = 2, top: int = 0):
+ """Вырезать фигуру из компоненты, растёкшейся по фону.
+
+ Гладкая стена тоннеля связна по глубине от ближнего поля до горизонта:
+ соседние лучи отличаются на сантиметры, и допуск сшивает их в одну
+ компоненту длиной сто метров. Предмет у такой стены попадает в неё же
+ и вместе с ней отбрасывается правилом `max_depth` — система слепнет
+ именно там, где смотрит вдоль стены, а это опаснее лишней тревоги.
+
+ Разорвать такую компоненту по дальности нельзя: предмет и стена рядом с
+ ним стоят на одной дальности. Зато они по-разному **приближаются**. Вдоль
+ фиксированного луча стена, параллельная движению, не приближается вовсе:
+ поезд едет, точка пересечения скользит по стене, дальность не меняется.
+ Предмет, обращённый к поезду, приближается ровно на пройденный путь.
+ Отсюда `advance` ≈ 0 у фона и ≈ 1 у фигуры.
+
+ Это и есть разделение фигуры и фона, которым у мухи занят канал
+ T4/T5 → LPTC: широкопольный поток задаёт ожидание, а то, что движется
+ иначе, становится фигурой.
+
+ Второй, более сильный источник того же признака — **контраст ламины**.
+ Гладкая стена по построению даёт нулевой центр-окружение: сколько бы ни
+ менялась дальность вдоль стены, она меняется плавно, и центр равен своему
+ окружению. Предмет на стене — это ступенька, и она видна. Замер на
+ вставленном человеке (`roundT_pressureGate_roundT` и `roundT_doubleT`):
+ при пороге 6 м порог проходят 85 % и 75 % лучей предмета против 0.13 % и
+ 0.22 % лучей фона, а сам фон распадается всего на 5–9 компонент в кадре
+ вместо 1259 у разреза по допуску.
+
+ Поэтому `figure` — любой покадровый признак «это не фон»: скорость
+ сближения вдоль луча или контраст к окружению в метрах. Пересобираются
+ только переглубокие компоненты и только по лучам с `figure > thr`;
+ остальные метки не трогаются.
+
+ `top` ограничивает число фигур, выносимых из одной компоненты, оставляя
+ сильнейшие по признаку. Это не косметика, а необходимость: разрез отрезает
+ фон, и протяжённость кандидата вдоль пути падает с 6.9 до 0.8 м — вместе с
+ ней исчезает множитель компактности в весе улики, который до того давил
+ конструкции в десять раз. Каждая лишняя фигура поэтому стоит дорого.
+ Ограничение — тот же приём глобального торможения, которым APL оставляет
+ активными считанные проценты клеток Кеньона. 0 — без ограничения.
+ """
+ if n == 0 or figure is None:
+ return labels, n
+ flat = labels.ravel()
+ rr = r.ravel()
+ adv = figure.ravel()
+ order = np.argsort(flat, kind="stable")
+ starts = np.searchsorted(flat[order], np.arange(n + 2))
+
+ out, next_id, copied = labels, n, False
+ for lbl in range(1, n + 1):
+ idx = order[starts[lbl]:starts[lbl + 1]]
+ if idx.size < 2:
+ continue
+ v = rr[idx]
+ v = v[np.isfinite(v)]
+ if v.size < 2 or float(v.max() - v.min()) <= max_depth:
+ continue
+
+ keep = idx[adv[idx] > thr]
+ if keep.size < 4 or keep.size == idx.size:
+ continue
+ sub = np.zeros(labels.shape, bool)
+ sub.ravel()[keep] = True
+ parts, n2 = cluster_by_depth(sub, r, col_reach=col_reach, row_reach=row_reach)
+ if n2 == 0:
+ continue
+ lbl_of = parts.ravel()[keep]
+ if top > 0 and n2 > top:
+ # сильнейшие по признаку: сумма превышения порога по всем лучам
+ w = np.bincount(lbl_of, weights=adv[keep] - thr, minlength=n2 + 1)
+ best = np.argsort(w[1:])[::-1][:top] + 1
+ alive = np.isin(lbl_of, best)
+ keep, lbl_of = keep[alive], lbl_of[alive]
+ if keep.size < 4:
+ continue
+ # перенумеровать оставшиеся подряд
+ _, lbl_of = np.unique(lbl_of, return_inverse=True)
+ lbl_of += 1
+ n2 = int(lbl_of.max())
+ if not copied:
+ out, copied = labels.copy(), True
+ # фон этой компоненты уходит из рассмотрения: он и был бы отброшен
+ out.ravel()[idx] = 0
+ out.ravel()[keep] = next_id + lbl_of
+ next_id += n2
+ return out, next_id
+
+
+def _core_zone(abs_lat: np.ndarray, d: np.ndarray, core_width: float,
+ core_from: float) -> np.ndarray:
+ """Где действует пониженный пол: в колее и не ближе core_from."""
+ zone = abs_lat <= core_width
+ if core_from > 0.0:
+ with np.errstate(invalid="ignore"):
+ zone = zone & (d >= core_from)
+ return zone
+
+
+def gauge_mask(tf: TrackFrame, corridor: Corridor, *, half_width: float,
+ h_lo: float, h_hi: float, d_min: float, d_max: float,
+ k_sigma: float = 0.0, h_lo_core: float = 1e9,
+ core_width: float = 0.85, core_from: float = 0.0,
+ platform_filter: bool = False,
+ platform_u: float = 1.30, platform_h_lo: float = 1.05,
+ platform_h_hi: float = 1.25,
+ h_top: float = 0.0, half_width_top: float = 0.0,
+ top_d_max: float = 90.0):
+ """Лучи, попавшие в габарит приближения. Считается один раз на кадр.
+
+ Нужны и для выделения кандидатов, и для накопителя веерного тела, поэтому
+ вынесены отдельно: пересчитывать один и тот же набор масок дважды незачем.
+
+ Верхняя секция (`h_top` > `h_hi`) — от `h_hi` до `h_top`, уже основной:
+ |u| < `half_width_top`. Она ловит то, что свисает со свода в путь вагона,
+ и не цепляет кабели и светильники на стенах, которые у круглого свода на
+ этой высоте подходят к оси ближе полуширины габарита. Дальше `top_d_max`
+ её нет: там ошибка наклона плоскости и оси пути поднимает и сдвигает к
+ оси конструкции свода, и на пустых записях они шли в кандидаты сотнями.
+ """
+ lat = tf.lateral(corridor)
+ abs_lat = np.abs(lat)
+ in_range = tf.valid & (tf.d > d_min) & (tf.d < d_max)
+
+ if k_sigma > 0.0:
+ sig = corridor.sigma(tf.d)
+ w_eff = half_width + k_sigma * np.maximum(sig - 0.25, 0.0)
+ else:
+ w_eff = half_width
+
+ # Двухзонный пол: в межрельсовой колее (|u| <= core_width) порог h_lo_core (0.16 м),
+ # чтобы не терять лежащего человека или низкие предметы. Опускается он
+ # только дальше core_from: вблизи в полосу 0.16…0.28 м попадают головки
+ # рельсов, и предмет теряется вместе с ними (EXPERIMENTS п. 16).
+ floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
+ min(h_lo_core, h_lo), h_lo)
+
+ inside = in_range & (abs_lat < w_eff) & (tf.h > floor_h) & (tf.h < h_hi)
+ if h_top > h_hi and half_width_top > 0.0:
+ inside |= (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top)
+ & (tf.h >= h_hi) & (tf.h < h_top))
+
+ if platform_filter:
+ platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
+ inside = inside & ~platform
+
+ return inside, lat, in_range
+
+
+def find_candidates(tf: TrackFrame, lam: LaminaOutput, corridor: Corridor, *,
+ half_width: float = 1.7, h_lo: float = 0.28, h_hi: float = 2.3,
+ d_min: float = 4.0, d_max: float = 220.0,
+ min_rays: int = 4, min_rays_far: int = 0,
+ min_rays_far_from: float = 90.0,
+ max_candidates: int = 64,
+ dilate_cols: int = 2, max_depth: float = 15.0,
+ ctx_up: float = 4.0, split_adv: float = 0.0,
+ split_gap: float = 0.0, split_near: float = 0.0,
+ near_long: float = 0.0,
+ split_top: int = 0,
+ advance: np.ndarray | None = None,
+ masks: tuple | None = None,
+ k_sigma: float = 0.0, h_lo_core: float = 1e9,
+ core_width: float = 0.85, core_from: float = 0.0,
+ platform_filter: bool = False,
+ platform_u: float = 1.30, platform_h_lo: float = 1.05,
+ platform_h_hi: float = 1.25,
+ h_top: float = 0.0, half_width_top: float = 0.0,
+ top_d_max: float = 90.0, top_detect: bool = False,
+ top_min_rays: int = 3, cluster=None) -> list[Candidate]:
+ """Выделить кандидатов: связные пятна лучей, попавших в габарит.
+
+ Кластеризация ведётся по **расширенной** области, а членство в габарите
+ проверяется уже для готового пятна. Без этого вертикальный лоскут стены,
+ срезанный границей коридора, неотличим от предмета: обрезка сама создаёт
+ компактное пятно нужного размера. Доля лучей, оставшихся внутри габарита
+ (`containment`), сразу показывает, предмет это целиком или край стены.
+ """
+ if masks is None:
+ masks = gauge_mask(tf, corridor, half_width=half_width, h_lo=h_lo,
+ h_hi=h_hi, d_min=d_min, d_max=d_max,
+ k_sigma=k_sigma, h_lo_core=h_lo_core,
+ core_width=core_width, core_from=core_from,
+ platform_filter=platform_filter,
+ platform_u=platform_u, platform_h_lo=platform_h_lo,
+ platform_h_hi=platform_h_hi)
+ inside, lat, in_range = masks
+ if not inside.any():
+ return []
+
+ # Контекст расширяется в стороны и вверх, но НЕ вниз: полотно пути проходит
+ # прямо под каждым предметом и на большой дальности попадает в тот же допуск
+ # по глубине, так что расширение вниз склеило бы любой объект с полом.
+ # Вверх, наоборот, нужно доставать до свода: колонна и рама крепи идут от
+ # полотна до потолка, и если обрезать контекст на полпути, срез сам выглядит
+ # компактным предметом ростом с габарит.
+ abs_lat = np.abs(lat)
+ if k_sigma > 0.0:
+ sig = corridor.sigma(tf.d)
+ w_ctx = half_width + 1.6 + k_sigma * np.maximum(sig - 0.25, 0.0)
+ else:
+ w_ctx = half_width + 1.6
+
+ floor_h = np.where(_core_zone(abs_lat, tf.d, core_width, core_from),
+ min(h_lo_core, h_lo), h_lo)
+ context = (in_range & (abs_lat < w_ctx)
+ & (tf.h > floor_h) & (tf.h < h_hi + ctx_up))
+
+ if platform_filter:
+ platform = (abs_lat >= platform_u) & (tf.h >= platform_h_lo) & (tf.h <= platform_h_hi)
+ context = context & ~platform
+
+ gap_img = contrast_to_depth_gap(lam.on, tf.r)
+
+ # `cluster` — та же кластеризация на видеокарте (FlyGuard._cluster), с тем
+ # же результатом; здесь самая большая маска кадра, и выигрыш в ней
+ labels, n = (cluster or cluster_by_depth)(context, tf.r, col_reach=dilate_cols + 1)
+ if n == 0:
+ return []
+ if split_gap > 0:
+ # Контраст к окружению как признак фигуры. Если задан и порог движения,
+ # выполняются оба условия сразу: стена должна и приближаться как предмет,
+ # и торчать из своего окружения.
+ fig = gap_img if (split_adv <= 0 or advance is None) else np.where(advance > split_adv, gap_img, 0.0)
+ if split_near > 0:
+ # Ближе этой дальности разрез был выключен целиком, и причина
+ # стояла в комментарии неверная: считалось, что вблизи предмет со
+ # стеной не слипается. Замер показал обратное — на 6…30 м предмет,
+ # смещённый от оси на 0.9 м, теряется в 34 % наблюдений против 1 %
+ # на самой оси, и хуже всего там, где есть платформа. Он уходит
+ # вместе с её кромкой.
+ #
+ # Резать вблизи всё подряд дорого: обстановки, дающей контраст, в
+ # ближнем поле на порядок больше. Поэтому режем только компоненты,
+ # СЛИШКОМ РАСТЯНУТЫЕ по глубине, чтобы быть предметом: человек
+ # занимает по дальности десятки сантиметров, кромка платформы и
+ # стена — десятки метров. Мерой служит разброс дальности внутри
+ # компоненты (две свёртки по меткам, доли миллисекунды).
+ near = tf.d <= split_near
+ if near_long > 0.0 and n > 0:
+ lab = labels.ravel()
+ rr = np.where(np.isfinite(tf.r), tf.r, 0.0).ravel()
+ cnt = np.bincount(lab, minlength=n + 1).astype(np.float64)
+ s1 = np.bincount(lab, weights=rr, minlength=n + 1)
+ s2 = np.bincount(lab, weights=rr * rr, minlength=n + 1)
+ c = np.maximum(cnt, 1.0)
+ var = np.maximum(s2 / c - (s1 / c) ** 2, 0.0)
+ stretched = np.sqrt(var) > near_long
+ stretched[0] = False
+ near = near & ~stretched[labels]
+ fig = np.where(near, 0.0, fig)
+ labels, n = split_by_figure(labels, n, tf.r, fig, max_depth=max_depth,
+ thr=split_gap, col_reach=dilate_cols + 1,
+ top=split_top)
+ elif split_adv > 0 and advance is not None:
+ labels, n = split_by_figure(labels, n, tf.r, advance, max_depth=max_depth,
+ thr=split_adv, col_reach=dilate_cols + 1)
+
+ flat = labels.ravel()
+ counts = np.bincount(flat, minlength=n + 1)
+ counts[0] = 0
+ inner = np.bincount(flat, weights=inside.ravel().astype(np.float64), minlength=n + 1)
+ inner[0] = 0
+ # Сколько лучей достаточно, чтобы считать компоненту кандидатом. Вблизи
+ # обрывок из трёх лучей — это кусок чего-то большего, и он только
+ # дробит настоящий предмет: замер даёт P@50 у человека 0.70 → 0.64,
+ # если разрешить тройки везде. Вдали три луча — всё, что вообще есть
+ # (медиана на 150 м — шесть), и запрет отсекает предмет целиком.
+ # Поэтому порог зависит от дальности, как и нормировка числа лучей в
+ # `_quality`. 0 — не различать (EXPERIMENTS п. 12.5).
+ need = np.full(inner.shape, float(min_rays))
+ if min_rays_far > 0:
+ din = np.where(inside, np.nan_to_num(tf.d, nan=0.0, posinf=0.0), 0.0)
+ dsum = np.bincount(flat, weights=din.ravel().astype(np.float64),
+ minlength=n + 1)
+ dmean = dsum / np.maximum(inner, 1.0)
+ need = np.where(dmean >= min_rays_far_from, float(min_rays_far),
+ float(min_rays))
+ keep = np.flatnonzero(inner >= need)
+ if keep.size == 0:
+ return []
+ if keep.size > max_candidates:
+ keep = keep[np.argsort(inner[keep])[::-1][:max_candidates]]
+
+ with np.errstate(invalid="ignore"):
+ deficit = np.where(np.isfinite(tf.floor_r), tf.floor_r - tf.r, 0.0)
+ deficit = np.where(tf.valid & (deficit > 0), deficit, 0.0).astype(np.float32)
+
+ order = np.argsort(labels.ravel(), kind="stable")
+ sorted_lbl = labels.ravel()[order]
+ starts = np.searchsorted(sorted_lbl, np.arange(n + 2))
+
+ width_img = labels.shape[1]
+
+ def build(sel: np.ndarray, n_ctx: int) -> Candidate | None:
+ """Кандидат по лучам ядра `sel` (плоские индексы) из компоненты в `n_ctx` лучей."""
+ ii, jj = np.divmod(sel, width_img)
+ d = tf.d[ii, jj]
+ u = lat[ii, jj]
+ h = tf.h[ii, jj]
+ nn = d.size
+ depth = float(d.max() - d.min()) if nn > 1 else 0.0
+ if depth > max_depth:
+ # ни один посторонний предмет не тянется на десятки метров вдоль пути:
+ # это полотно, стена или кабельный лоток, взятые под скользящим углом
+ return None
+ cand = Candidate(
+ d=float(np.median(d)), u=float(np.median(u)), h=float(np.median(h)),
+ d_min=float(d.min()), h_min=float(h.min()),
+ width=float(u.max() - u.min()) if nn > 1 else 0.0,
+ height=float(h.max() - h.min()) if nn > 1 else 0.0,
+ depth=depth,
+ containment=float(sel.size / max(n_ctx, 1)),
+ n_rays=int(nn), n_rings=int(np.unique(ii).size), n_cols=int(np.unique(jj).size),
+ gap=float(np.median(gap_img[ii, jj])),
+ on=float(np.median(lam.on[ii, jj])),
+ floor_deficit=float(np.median(deficit[ii, jj])),
+ shadow=float(np.median(lam.hole[ii, jj])),
+ inten=float(np.median(tf.inten[ii, jj])),
+ az_deg=float(np.mean(tf.layout.az_grid_deg[jj])),
+ el_deg=float(np.mean(tf.layout.el_deg[ii])),
+ bbox=(int(ii.min()), int(ii.max()), int(jj.min()), int(jj.max())),
+ )
+ # индексы лучей нужны только для отладочной визуализации; они небольшие
+ cand.extra["rays"] = (ii, jj)
+ # Смещение в системе лидара, без поправки на кривую: `u` отсчитан от
+ # оси пути, и в кривой рамка по нему встала бы в стороне от своих
+ # точек (на синтетике — в 1.2 м на 39 м). Нужно только для рисования.
+ cand.extra["u_raw"] = float(np.median(tf.u[ii, jj]))
+ return cand
+
+ use_top = top_detect and h_top > h_hi and half_width_top > 0.0
+ h_flat = tf.h.ravel()
+ inside_flat = inside.ravel()
+ out: list[Candidate] = []
+ for lbl in keep:
+ ctx_idx = order[starts[lbl]:starts[lbl + 1]]
+ sel = ctx_idx[inside_flat[ctx_idx]] # признаки считаем по ядру в габарите
+ if sel.size < need[lbl]:
+ continue
+ if use_top and np.all(h_flat[sel] >= h_hi):
+ # целиком в верхней секции: это работа отдельного прохода ниже —
+ # здесь такой кандидат склеен со сводом и только двоил бы трек
+ continue
+ cand = build(sel, ctx_idx.size)
+ if cand is not None:
+ out.append(cand)
+
+ if use_top:
+ out += _hanging(tf, abs_lat, in_range, build, h_hi=h_hi, h_top=h_top,
+ half_width_top=half_width_top, top_d_max=top_d_max,
+ min_rays=top_min_rays, col_reach=dilate_cols + 1)
+
+ out.sort(key=lambda c: c.d)
+ return out
+
+
+def _hanging(tf: TrackFrame, abs_lat, in_range, build, *, h_hi: float, h_top: float,
+ half_width_top: float, top_d_max: float, min_rays: int,
+ col_reach: int, margin_u: float = 0.3, below: float = 0.4) -> list[Candidate]:
+ """Свисающее со свода в путь вагона: отдельный проход по верхней секции.
+
+ Основной проход тянет контекст до свода, чтобы колонна не выглядела
+ предметом, — и поэтому всё, что свисает с потолка, склеивается со сводом:
+ доля лучей в габарите у стержня из синтетики организаторов была ноль.
+ Просто не пускать свод в контекст нельзя: тогда освобождаются и стоящие
+ конструкции, прицепленные к своду, — на `roundT_doubleT` так вышла тревога
+ на светофоре у стрелки. Поэтому здесь свой проход: контекст — только сама
+ верхняя секция с запасом `margin_u` по сторонам и `below` вниз. Компонента,
+ у которой есть лучи ниже `h_hi`, продолжается к полу: это стоящий предмет
+ (столб, светофор, человек высокого роста), и его ведёт основной проход.
+ Висящему не нужна опора снизу, и обученное считывание его не судит
+ (см. central_complex._quality, `top_from`). EXPERIMENTS п. 19.
+ """
+ band = (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top)
+ & (tf.h >= h_hi) & (tf.h < h_top))
+ if not band.any():
+ return []
+ ext = (in_range & (tf.d < top_d_max) & (abs_lat < half_width_top + margin_u)
+ & (tf.h > h_hi - below) & (tf.h < h_top))
+ labels, n = cluster_by_depth(ext, tf.r, col_reach=col_reach)
+ if n == 0:
+ return []
+ flat_lbl = labels.ravel()
+ order = np.argsort(flat_lbl, kind="stable")
+ starts = np.searchsorted(flat_lbl[order], np.arange(n + 2))
+ band_flat = band.ravel()
+ low_flat = (tf.h < h_hi).ravel()
+ out = []
+ for lbl in range(1, n + 1):
+ idx = order[starts[lbl]:starts[lbl + 1]]
+ if idx.size < min_rays or low_flat[idx].any():
+ continue
+ sel = idx[band_flat[idx]]
+ if sel.size < min_rays:
+ continue
+ cand = build(sel, idx.size)
+ if cand is None:
+ continue
+ if cand.height < 0.1 and cand.h_min > h_top - 0.2:
+ # Плоское у самой верхней границы — это свод: на 80–90 м ошибка
+ # наклона плоскости пути опускает его в секцию (на `squareT_…`
+ # так вышла тревога на 83 м, высота 3.66–3.69 м при границе 3.7).
+ # Свисающее начинается ниже и имеет высоту.
+ continue
+ cand.extra["top"] = True
+ out.append(cand)
+ return out
diff --git a/ros2_ws/src/flyguard/flyguard/mbon_readout.py b/ros2_ws/src/flyguard/flyguard/mbon_readout.py
new file mode 100644
index 0000000..61eee8f
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/mbon_readout.py
@@ -0,0 +1,390 @@
+"""MBON с обучением с учителем — считывание, а не новая сеть.
+
+Грибовидное тело в `mushroom_body.py` учится **без меток**: синапсы KC→MBON
+депрессируются на всём, что тоннель показывает часто, и выход MBON означает
+«незнакомо». Это ровно familiarity suppression MBON-α′3 и ровно то, что нужно,
+когда меток нет.
+
+Но у мухи та же схема умеет и другое. При обучении с подкреплением
+дофаминергические нейроны PPL1/PAM депрессируют KC→MBON **избирательно** — те
+клетки, что были активны вместе с наказанием, — и выход MBON начинает означать
+не «незнакомо», а «это предвещает удар». Один и тот же нейропиль, один и тот же
+разрежённый код, другой учитель.
+
+Здесь сделано именно это. Слои не меняются:
+
+* вход — те же признаки кандидата плюс опора веерного тела (23 «проекционных
+ нейрона»);
+* KC — та же случайная разрежённая проекция по 6 «когтей» на клетку;
+* APL — то же глобальное торможение «победитель забирает всё», но отклик
+ остаётся **градуальным** и делится на общую активность (дивизивная
+ нормировка), а не превращается в единицы и нули;
+* MBON — один выход, веса которого обучены различать предмет и обстановку.
+
+Метки берутся не из разметки (её нет), а из физики: `flyguard.synth` вставляет
+предмет трассировкой лучей, и кандидат считается предметом, если его ядро
+состоит из лучей, в которые предмет действительно записан
+(`tools/make_training_set.py`).
+
+Зачем это нужно поверх ручной формулы веса улики. В `central_complex._quality`
+шесть множителей, придуманных руками, а в дескрипторе 23 признака: **тень и
+интенсивность в вес улики не входят вообще**, хотя окклюзионная тень за
+предметом на большой дальности во много раз крупнее самого предмета.
+
+Стоимость в инференсе — одно умножение матрицы 23 × n_kc на кандидата, доли
+миллисекунды на CPU. GPU нужен только на обучении, и то не обязателен.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+
+from .lobula import Candidate
+from .mushroom_body import describe
+
+
+@dataclass
+class MbonConfig:
+ """Параметры считывания."""
+
+ # Ёмкость выбирается замером (`tools/train_mbon.py --sweep-kc`), а не на
+ # глаз: отбор «победитель забирает всё» стоит дороже матмула, и в худшем
+ # кадре с 64 кандидатами 20 000 клеток это 7.3 мс, 8 000 — 2.8 мс, 4 000 —
+ # 1.3 мс. Умолчание взято средним: платить временем кадра за ёмкость имеет
+ # смысл, только если развёртка показала, что она что-то даёт.
+ n_kc: int = 8_000 # клеток Кеньона в этом контуре
+ claws: int = 6 # входов на клетку (из коннектома)
+ sparsity: float = 0.0125 # доля активных после торможения APL — 100 клеток
+ seed: int = 20260921
+
+
+def describe_full(c: Candidate) -> np.ndarray:
+ """Вектор кандидата для считывания: дескриптор памяти плюс опора накопителя."""
+ acc = float(c.extra.get("acc_support", 0.0)) if c.extra else 0.0
+ return np.append(describe(c), np.float32(acc)).astype(np.float32)
+
+
+class MbonReadout:
+ """Разрежённый код клеток Кеньона + обученный линейный выход MBON."""
+
+ def __init__(self, cfg: MbonConfig | None = None, n_pn: int = 23):
+ self.cfg = cfg or MbonConfig()
+ self.n_pn = int(n_pn)
+ self.n_active = max(1, int(round(self.cfg.n_kc * self.cfg.sparsity)))
+ rng = np.random.default_rng(self.cfg.seed)
+ idx = np.stack([rng.choice(self.n_pn, size=self.cfg.claws, replace=False)
+ for _ in range(self.cfg.n_kc)])
+ sign = rng.choice((-1.0, 1.0), size=idx.shape)
+ w = np.zeros((self.cfg.n_kc, self.n_pn), np.float32)
+ np.put_along_axis(w, idx, sign.astype(np.float32), axis=1)
+ self.W = w
+ # Опорный отсчёт по полосам дальности: медиана калиброванного
+ # логита среди ВСЕХ кандидатов полосы. Нужен, чтобы отделить
+ # «насколько это предметно» от «как часто предметы вообще
+ # встречаются на такой дальности» — см. _prior_corr.
+ self.ref_edges = np.array([0., 30., 55., 80., 110., 160., 230.],
+ np.float32)
+ self.ref_z: np.ndarray | None = None
+ self.mean = np.zeros(self.n_pn, np.float32)
+ self.scale = np.ones(self.n_pn, np.float32)
+ self.w_mbon = np.zeros(self.cfg.n_kc, np.float32)
+ self.bias = np.float32(0.0)
+ # калибровка выхода: sigmoid(gain·z + shift). Градиентный спуск по
+ # разрежённому коду хорошо упорядочивает кандидатов, но масштаб логита
+ # зависит от ёмкости и числа эпох, а нам нужна осмысленная вероятность —
+ # она идёт множителем в вес улики.
+ self.gain = np.float32(1.0)
+ self.shift = np.float32(0.0)
+
+ # ------------------------------------------------------------------ код
+
+ def fit_normalizer(self, X: np.ndarray) -> None:
+ X = np.atleast_2d(np.asarray(X, np.float32))
+ self.mean = X.mean(0).astype(np.float32)
+ s = X.std(0).astype(np.float32)
+ self.scale = np.where(s > 1e-6, s, 1.0).astype(np.float32)
+
+ def encode(self, X: np.ndarray, device: str | None = None):
+ """Признаки → (индексы активных клеток, их нормированный отклик).
+
+ Отклик остаётся градуальным и делится на свою сумму: это дивизивная
+ нормировка APL. Двоичный код тут заметно хуже — он выбрасывает
+ «насколько» клетка возбуждена, а для решения это как раз важно.
+
+ Нормировка приводит средний отклик к единице, а не сумму: иначе каждая
+ из сотни активных клеток даёт вклад 0.01, логиты выходят микроскопические
+ и обучение сваливается в разумное ранжирование при бессмысленной
+ калибровке (замерено: AUC 0.97 при доле верных 0.37).
+ """
+ X = np.atleast_2d(np.asarray(X, np.float32))
+ if X.shape[1] != self.n_pn:
+ raise ValueError(f"считывание обучено на {self.n_pn} признаках, "
+ f"а дескриптор даёт {X.shape[1]}")
+ k = self.n_active
+ chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
+
+ if device == "auto":
+ from .device import get_device
+ device = get_device("auto")
+ if device and device != "cpu":
+ try:
+ import torch
+ out_i = np.empty((X.shape[0], k), np.int64)
+ out_v = np.empty((X.shape[0], k), np.float32)
+ with torch.no_grad():
+ m = torch.as_tensor(self.mean, device=device)
+ s = torch.as_tensor(self.scale, device=device)
+ w = torch.as_tensor(self.W, device=device).T.contiguous()
+ for i in range(0, X.shape[0], chunk):
+ t = torch.as_tensor(X[i:i + chunk], device=device)
+ y = torch.relu(((t - m) / s) @ w)
+ v, a = torch.topk(y, k, dim=1)
+ v = v * (k / v.sum(1, keepdim=True).clamp_min(1e-6))
+ out_i[i:i + chunk] = a.cpu().numpy()
+ out_v[i:i + chunk] = v.cpu().numpy()
+ return out_i, out_v
+ except Exception as e:
+ from .device import notify_cuda_error
+ notify_cuda_error(e)
+ # Переход к расчету на CPU
+
+ out_i = np.empty((X.shape[0], k), np.int64)
+ out_v = np.empty((X.shape[0], k), np.float32)
+ for i in range(0, X.shape[0], chunk):
+ z = (X[i:i + chunk] - self.mean) / self.scale
+ y = np.maximum(z @ self.W.T, 0.0)
+ a = np.argpartition(-y, k - 1, axis=1)[:, :k]
+ v = np.take_along_axis(y, a, axis=1)
+ v *= k / np.maximum(v.sum(1, keepdims=True), 1e-6)
+ out_i[i:i + chunk] = a
+ out_v[i:i + chunk] = v
+ return out_i, out_v
+
+ # ------------------------------------------------------------------ выход
+
+ def score(self, X: np.ndarray) -> np.ndarray:
+ """Вероятность «это посторонний предмет», 0…1."""
+ a, v = self.encode(X)
+ z = self.logit(X, code=(a, v))
+ return 1.0 / (1.0 + np.exp(-z))
+
+ def logit(self, X: np.ndarray, code=None) -> np.ndarray:
+ a, v = code if code is not None else self.encode(X)
+ z = self.bias + (self.w_mbon[a] * v).sum(axis=1)
+ return self.gain * z + self.shift
+
+ def score_of(self, c: Candidate) -> float:
+ return float(self.score(describe_full(c)[None, :])[0])
+
+ def _ref_at(self, d: np.ndarray) -> np.ndarray:
+ """Типичный отсчёт модели на дальности d (по обучающей выборке)."""
+ mid = 0.5 * (self.ref_edges[:-1] + self.ref_edges[1:])
+ return np.interp(d, mid, self.ref_z)
+
+ def _prior_corr(self, d: np.ndarray, prior_from: float) -> np.ndarray:
+ """Поправка на распространённость предметов: логит → отношение.
+
+ Модель калибрована в апостериорную вероятность, а доля предметов
+ среди кандидатов падает с 42 % вблизи до 3.1 % за 160 м. Поэтому
+ честная вероятность на дальности низка даже у настоящего предмета:
+ замер даёт 0.051 у вставленного человека на 150 м — при 0.003 у
+ обстановки, то есть модель РАЗЛИЧАЕТ (AUC 0.743), но абсолютная
+ величина раздавлена редкостью. Детектору нужно отношение
+ правдоподобий, а не доля, поэтому из логита вычитается типичный
+ для этой полосы — ровно как число лучей уже нормируется на
+ ожидаемое для дальности. Ближе `prior_from` поправка нулевая.
+ """
+ if self.ref_z is None or prior_from <= 0.0:
+ return np.zeros_like(d)
+ base = float(self._ref_at(np.array([prior_from], np.float32))[0])
+ corr = base - self._ref_at(d)
+ return np.where(d > prior_from, np.maximum(corr, 0.0), 0.0)
+
+ def annotate(self, cands: list[Candidate], prior_from: float = 0.0
+ ) -> list[Candidate]:
+ if not cands:
+ return cands
+ X = np.stack([describe_full(c) for c in cands])
+ z = self.logit(X)
+ if prior_from > 0.0:
+ d = np.array([c.d for c in cands], np.float32)
+ z = z + self._prior_corr(d, prior_from)
+ for c, zz in zip(cands, z):
+ c.extra["mbon"] = float(1.0 / (1.0 + np.exp(-zz)))
+ return cands
+
+ # ------------------------------------------------------------------ обучение
+
+ def _learn_cpu(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
+ lr: float = 4.0, l2: float = 1e-5, verbose: bool = False) -> None:
+ """Обучение на CPU через NumPy."""
+ y = np.asarray(y, np.float32)
+ a, v = self.encode(X, device="cpu")
+ n, k = a.shape
+ flat = a.ravel()
+ for ep in range(epochs):
+ z = self.bias + (self.w_mbon[a] * v).sum(axis=1)
+ p = 1.0 / (1.0 + np.exp(-z))
+ g = (p - y) / n
+ grad = np.bincount(flat, weights=np.repeat(g, k) * v.ravel(),
+ minlength=self.cfg.n_kc).astype(np.float32)
+ self.w_mbon -= lr * (grad + l2 * self.w_mbon)
+ self.bias -= np.float32(lr * g.sum())
+ if verbose and (ep + 1) % 50 == 0:
+ eps = 1e-7
+ loss = -(y * np.log(p + eps) + (1 - y) * np.log(1 - p + eps)).mean()
+ print(f" эпоха {ep + 1:4d}: логистическая потеря {loss:.4f}")
+ zr = self.bias + (self.w_mbon[a] * v).sum(axis=1)
+ self._calibrate(zr, y)
+ zc = self.gain * zr + self.shift
+ dd = np.exp(np.asarray(X[:, 0], np.float64))
+ ref = []
+ for lo, hi in zip(self.ref_edges[:-1], self.ref_edges[1:]):
+ m = (dd >= lo) & (dd < hi)
+ ref.append(float(np.median(zc[m])) if m.sum() >= 50
+ else (ref[-1] if ref else 0.0))
+ self.ref_z = np.array(ref, np.float32)
+
+ def _learn_gpu(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
+ lr: float = 4.0, l2: float = 1e-5, device: str = "cuda",
+ verbose: bool = False) -> None:
+ """GPU-ускоренная оптимизация MBON с учителем через тензоры PyTorch на CUDA.
+
+ Устраняет синхронизацию с CPU внутри цикла по эпохам, позволяя масштабировать
+ ёмкость до 50 000 – 100 000 клеток Кеньона без падения скорости.
+ """
+ import torch
+ dev = torch.device(device)
+ y_t = torch.as_tensor(y, dtype=torch.float32, device=dev)
+ n = y_t.shape[0]
+ k = self.n_active
+
+ m = torch.as_tensor(self.mean, dtype=torch.float32, device=dev)
+ s = torch.as_tensor(self.scale, dtype=torch.float32, device=dev)
+ w_proj = torch.as_tensor(self.W, dtype=torch.float32, device=dev).T.contiguous()
+
+ chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
+ a_list, v_list = [], []
+ with torch.no_grad():
+ for i in range(0, n, chunk):
+ t = torch.as_tensor(X[i:i + chunk], dtype=torch.float32, device=dev)
+ y_proj = torch.relu(((t - m) / s) @ w_proj)
+ v_chunk, a_chunk = torch.topk(y_proj, k, dim=1)
+ v_chunk = v_chunk * (k / v_chunk.sum(1, keepdim=True).clamp_min(1e-6))
+ a_list.append(a_chunk)
+ v_list.append(v_chunk)
+
+ a_t = torch.cat(a_list, dim=0) if len(a_list) > 1 else a_list[0]
+ v_t = torch.cat(v_list, dim=0) if len(v_list) > 1 else v_list[0]
+
+ w_mbon_t = torch.as_tensor(self.w_mbon, dtype=torch.float32, device=dev)
+ bias_t = torch.tensor(self.bias, dtype=torch.float32, device=dev)
+ flat_a = a_t.reshape(-1)
+
+ for ep in range(epochs):
+ z = bias_t + torch.sum(w_mbon_t[a_t] * v_t, dim=1)
+ p = torch.sigmoid(z)
+ g = (p - y_t) / n
+ weights = (g.unsqueeze(1) * v_t).reshape(-1)
+ grad = torch.zeros(self.cfg.n_kc, dtype=torch.float32, device=dev)
+ grad.scatter_add_(0, flat_a, weights)
+ w_mbon_t -= lr * (grad + l2 * w_mbon_t)
+ bias_t -= lr * g.sum()
+
+ if verbose and (ep + 1) % 50 == 0:
+ loss = -(y_t * torch.log(p.clamp_min(1e-7)) + (1.0 - y_t) * torch.log((1.0 - p).clamp_min(1e-7))).mean()
+ print(f" [GPU] эпоха {ep + 1:4d}: логистическая потеря {loss.item():.4f}")
+
+ zr_t = bias_t + torch.sum(w_mbon_t[a_t] * v_t, dim=1)
+
+ # Калибровка Платта полностью на GPU
+ g_t = torch.tensor(1.0, dtype=torch.float32, device=dev)
+ sh_t = torch.tensor(0.0, dtype=torch.float32, device=dev)
+ for _ in range(400):
+ p_cal = torch.sigmoid(g_t * zr_t + sh_t)
+ e_cal = p_cal - y_t
+ g_t -= 2.0 * (e_cal * zr_t).mean() / torch.clamp_min((zr_t * zr_t).mean(), 1e-6)
+ sh_t -= 2.0 * e_cal.mean()
+
+ self.w_mbon = w_mbon_t.cpu().numpy()
+ self.bias = np.float32(bias_t.item())
+ self.gain = np.float32(g_t.item())
+ self.shift = np.float32(sh_t.item())
+
+ zr = zr_t.cpu().numpy()
+ zc = self.gain * zr + self.shift
+ dd = np.exp(np.asarray(X[:, 0], np.float64))
+ ref = []
+ for lo, hi in zip(self.ref_edges[:-1], self.ref_edges[1:]):
+ m_mask = (dd >= lo) & (dd < hi)
+ ref.append(float(np.median(zc[m_mask])) if m_mask.sum() >= 50
+ else (ref[-1] if ref else 0.0))
+ self.ref_z = np.array(ref, np.float32)
+
+ def learn(self, X: np.ndarray, y: np.ndarray, *, epochs: int = 60,
+ lr: float = 4.0, l2: float = 1e-5, device: str | None = None,
+ verbose: bool = False) -> None:
+ """Логистическая регрессия по разрежённому коду — депрессия с учителем.
+
+ Поддерживает прозрачное переключение между NVIDIA GPU (CUDA) и CPU.
+ """
+ target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
+ if target_dev == "auto":
+ from .device import get_device
+ target_dev = get_device("auto")
+
+ if target_dev.startswith("cuda"):
+ try:
+ self._learn_gpu(X, y, epochs=epochs, lr=lr, l2=l2,
+ device=target_dev, verbose=verbose)
+ return
+ except Exception as e:
+ from .device import notify_cuda_error
+ notify_cuda_error(e)
+ import logging
+ logging.getLogger("flyguard.mbon").warning("GPU learning failed (%s), fallback to CPU", e)
+
+ self._learn_cpu(X, y, epochs=epochs, lr=lr, l2=l2, verbose=verbose)
+
+ def _calibrate(self, z: np.ndarray, y: np.ndarray, iters: int = 400) -> None:
+ """Шкалирование Платта: подобрать наклон и сдвиг по обучающей выборке."""
+ g, sh = 1.0, 0.0
+ for _ in range(iters):
+ p = 1.0 / (1.0 + np.exp(-(g * z + sh)))
+ e = p - y
+ g -= 2.0 * float((e * z).mean()) / max(float((z * z).mean()), 1e-6)
+ sh -= 2.0 * float(e.mean())
+ self.gain, self.shift = np.float32(g), np.float32(sh)
+
+ # ------------------------------------------------------------------ хранение
+
+ def save(self, path: str | Path) -> None:
+ np.savez_compressed(path, w_mbon=self.w_mbon, bias=self.bias,
+ gain=self.gain, shift=self.shift,
+ mean=self.mean, scale=self.scale,
+ n_kc=self.cfg.n_kc, claws=self.cfg.claws,
+ sparsity=self.cfg.sparsity, seed=self.cfg.seed,
+ n_pn=self.n_pn, ref_edges=self.ref_edges,
+ ref_z=(self.ref_z if self.ref_z is not None
+ else np.zeros(0, np.float32)))
+
+ @classmethod
+ def load(cls, path: str | Path) -> "MbonReadout":
+ """Загрузка через `cls`, а не через имя класса: считывание по трекам —
+ тот же контур с другим входом, и оно наследуется отсюда целиком."""
+ d = np.load(path, allow_pickle=False)
+ cfg = MbonConfig(n_kc=int(d["n_kc"]), claws=int(d["claws"]),
+ sparsity=float(d["sparsity"]), seed=int(d["seed"]))
+ m = cls(cfg, n_pn=int(d["n_pn"]))
+ m.w_mbon = d["w_mbon"].astype(np.float32)
+ m.bias = np.float32(d["bias"])
+ m.gain = np.float32(d["gain"])
+ m.shift = np.float32(d["shift"])
+ m.mean = d["mean"].astype(np.float32)
+ m.scale = d["scale"].astype(np.float32)
+ if "ref_z" in d.files and d["ref_z"].size:
+ m.ref_edges = d["ref_edges"].astype(np.float32)
+ m.ref_z = d["ref_z"].astype(np.float32)
+ return m
diff --git a/ros2_ws/src/flyguard/flyguard/medulla.py b/ros2_ws/src/flyguard/flyguard/medulla.py
new file mode 100644
index 0000000..f42af66
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/medulla.py
@@ -0,0 +1,398 @@
+"""MEDULLA и LOBULA PLATE — движение: T4/T5, LPTC и LPLC2.
+
+Три схемы из коннектома, работающие подряд:
+
+* **T4/T5** — элементарные детекторы движения. Каждый тип существует в четырёх
+ подтипах, настроенных на четыре стороны света в поле зрения; T4 читает ON-канал,
+ T5 — OFF. Вычислительно это коррелятор Хассенштайна–Райхардта: сигнал одного
+ омматидия задерживается и умножается на сигнал соседнего, разность двух таких
+ произведений даёт направленный отклик.
+* **LPTC (HS/VS)** — широкопольные тангенциальные клетки лобулярной пластинки.
+ Каждая суммирует выход тысяч T4/T5 по своему рецептивному полю и тем самым
+ измеряет **собственное движение**. Здесь это критично: колёсной одометрии в
+ задаче нет, и скорость поезда неоткуда взять, кроме как из самого потока.
+* **LPLC2** — детектор надвигания. Его дендриты разложены на четыре слоя так,
+ что клетка отвечает только на поток, расходящийся **из центра её рецептивного
+ поля**, и подавляется однородным широкопольным потоком. То есть она отделяет
+ «на меня что-то летит» от «я сам двигаюсь». Выход идёт на гигантское волокно.
+
+Практически: продвижение поезда за кадр оценивается корреляцией продольного
+профиля тоннеля (устойчиво и дёшево), а T4/T5 и LPLC2 дают карту остаточного
+движения — того, что не объясняется собственным ходом.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+# шаг гистограммы продольного профиля, м
+PROFILE_BIN = 0.5
+PROFILE_MAX = 250.0
+MAX_SPEED = 35.0 # м/с, заведомо выше любого метропоезда
+MAX_ACCEL = 3.0 # м/с², предел разгона и экстренного торможения состава
+WARMUP_FRAMES = 4 # столько кадров оценка принимается как есть, без фильтра
+
+
+@dataclass
+class EgoMotion:
+ """Собственное движение за один кадр."""
+
+ ds: float # продвижение вперёд, м
+ speed: float # м/с
+ yaw_deg: float # поворот за кадр, °
+ conf: float # 0…1, качество корреляционного пика
+ dt: float
+
+ @property
+ def kmh(self) -> float:
+ return self.speed * 3.6
+
+
+PROFILE_MIN = 14.0 # ближняя зона в профиль не идёт: там сдвиг кадра не читается
+
+
+def longitudinal_profile(d: np.ndarray, valid: np.ndarray) -> np.ndarray:
+ """Продольная «подпись» тоннеля: сколько лучей оборвалось на каждой дальности.
+
+ Тюбинговые кольца, лотки, ниши и стыки дают ей богатый рисунок, поэтому
+ сдвиг профиля между кадрами читается как пройденный путь.
+
+ Две поправки, без которых корреляция залипает на нулевом сдвиге:
+ ближняя зона исключается (там на один метр пути приходятся тысячи лучей,
+ и её вклад подавляет всё остальное), а от профиля отнимается скользящее
+ среднее — остаётся только рисунок структур, без общей огибающей.
+ """
+ n = int(PROFILE_MAX / PROFILE_BIN)
+ dd = d[valid]
+ dd = dd[(dd > PROFILE_MIN) & (dd < PROFILE_MAX)]
+ if dd.size < 50:
+ return np.zeros(n, np.float32)
+ p = np.bincount((dd / PROFILE_BIN).astype(np.int32), minlength=n)[:n].astype(np.float32)
+ p = np.log1p(p)
+ k = 11 # ≈5 м — крупнее шага структур
+ kern = np.ones(k, np.float32) / k
+ env = np.convolve(p, kern, mode="same")
+ return (p - env).astype(np.float32)
+
+
+def _norm(x: np.ndarray) -> np.ndarray:
+ x = x - x.mean()
+ s = np.linalg.norm(x)
+ return x / s if s > 1e-6 else x
+
+
+def match_shift(prev: np.ndarray, cur: np.ndarray, max_bins: int,
+ prior_bins: float | None = None, prior_w: float = 0.03) -> tuple[float, float]:
+ """Сдвиг `cur` относительно `prev` по максимуму нормированной корреляции.
+
+ Профиль текущего кадра смещён к меньшим дальностям на пройденный путь,
+ поэтому ищется такой сдвиг k, при котором cur[i] ≈ prev[i + k], k ≥ 0.
+ Слабый приор по предыдущей скорости снимает неоднозначность на периодических
+ структурах вроде тюбинговых колец; его вес нормирован на диапазон поиска,
+ чтобы он подправлял выбор между близкими пиками, а не диктовал ответ.
+
+ Корреляция считается по общей части профилей, поэтому при больших сдвигах
+ выборка короче — нормировка на длину не даёт этому создать ложный уклон.
+ """
+ n = prev.size
+ ks = np.arange(0, max_bins + 1)
+ scores = np.empty(ks.size, np.float32)
+ for i, k in enumerate(ks):
+ m = n - k
+ scores[i] = float(np.dot(_norm(cur[:m]), _norm(prev[k:k + m])))
+ if prior_bins is not None and max_bins > 0:
+ scores = scores - prior_w * ((ks - prior_bins) / max_bins) ** 2
+
+ i = int(np.argmax(scores))
+ peak = float(scores[i])
+ if 0 < i < ks.size - 1: # уточнение параболой по трём точкам
+ y0, y1, y2 = scores[i - 1], scores[i], scores[i + 1]
+ den = y0 - 2 * y1 + y2
+ sub = 0.5 * (y0 - y2) / den if abs(den) > 1e-9 else 0.0
+ else:
+ sub = 0.0
+ return float(ks[i] + np.clip(sub, -1, 1)), peak
+
+
+class RayIndexer:
+ """Обратный поиск по решётке: направление → (кольцо, столбец).
+
+ Нужен, чтобы перепроецировать точки предыдущего кадра в текущую решётку.
+ Элевации каналов заданы убывающей таблицей, поэтому индекс кольца берётся
+ линейной интерполяцией по ней, а не делением на постоянный шаг.
+ """
+
+ def __init__(self, layout):
+ el = np.asarray(layout.el_deg, np.float64)
+ order = np.argsort(el)
+ self.el_sorted = el[order]
+ self.ring_sorted = order.astype(np.float64)
+ self.az0 = float(layout.az_grid_deg[0])
+ self.step = float(layout.az_step_deg)
+ self.n_az = int(layout.n_az)
+ self.n_rings = int(layout.n_rings)
+
+ def __call__(self, az_deg: np.ndarray, el_deg: np.ndarray):
+ col = np.rint((az_deg - self.az0) / self.step).astype(np.int32)
+ ring = np.rint(np.interp(el_deg, self.el_sorted, self.ring_sorted)).astype(np.int32)
+ ok = (col >= 0) & (col < self.n_az) & (ring >= 0) & (ring < self.n_rings)
+ np.clip(col, 0, self.n_az - 1, out=col)
+ np.clip(ring, 0, self.n_rings - 1, out=ring)
+ return ring, col, ok
+
+
+def advance_score(prev_pts: np.ndarray, r_cur: np.ndarray, valid_cur: np.ndarray,
+ indexer: RayIndexer, ds: float) -> float:
+ """Доля точек прошлого кадра, попавших в текущий кадр при сдвиге вперёд на ds.
+
+ Это и есть проверка широкопольного потока на согласие с моделью собственного
+ движения — то, чем заняты тангенциальные клетки лобулярной пластинки.
+ """
+ d = prev_pts[:, 0] - ds
+ u = prev_pts[:, 1]
+ h = prev_pts[:, 2]
+ m = d > 2.0
+ if m.sum() < 50:
+ return 0.0
+ d, u, h = d[m], u[m], h[m]
+ rho = np.hypot(d, u)
+ r = np.sqrt(rho * rho + h * h)
+ az = np.degrees(np.arctan2(u, d))
+ el = np.degrees(np.arctan2(h, rho))
+ ring, col, ok = indexer(az, el)
+ rc = r_cur[ring, col]
+ good = ok & valid_cur[ring, col]
+ if good.sum() < 50:
+ return 0.0
+ err = np.abs(rc[good] - r[good])
+ tol = np.maximum(0.25, 0.015 * r[good])
+ return float(np.mean(err < tol))
+
+
+class EgoMotionEstimator:
+ """LPTC-аналог: одна широкопольная оценка собственного движения на кадр."""
+
+ def __init__(self, dt_nominal: float = 0.1):
+ self.prev_profile: np.ndarray | None = None
+ self.prev_az: np.ndarray | None = None
+ self.prev_stamp: float | None = None
+ self.prev_ds: float | None = None
+ self.prev_pts: np.ndarray | None = None
+ self.indexer: RayIndexer | None = None
+ self.n_sample = 6000
+ self.v_filt: float | None = None
+ self.n_updates = 0
+ self.dt_nominal = dt_nominal
+
+ def update(self, tf, stamp: float) -> EgoMotion:
+ dt = self.dt_nominal
+ if self.prev_stamp is not None:
+ got = stamp - self.prev_stamp
+ if 0.01 < got < 1.0:
+ dt = got
+
+ prof = longitudinal_profile(tf.d, tf.valid)
+ az = np.log1p(tf.valid.sum(axis=0)).astype(np.float32)
+
+ ds, conf, yaw = 0.0, 0.0, 0.0
+ if self.prev_profile is not None and np.any(prof):
+ max_bins = int(MAX_SPEED * dt / PROFILE_BIN) + 2
+ prior = None if self.prev_ds is None else self.prev_ds / PROFILE_BIN
+ shift, conf = match_shift(self.prev_profile, prof, max_bins, prior_bins=prior)
+ ds = shift * PROFILE_BIN
+
+ # независимая грубая оценка: дальние фронтальные поверхности приближаются
+ # ровно на пройденный путь
+ direct = self._direct_advance(tf)
+ seeds = [s for s in (ds, direct, self.prev_ds, 0.0) if s is not None]
+
+ # уточнение сопоставлением кадров: перебор сдвига с проверкой согласия
+ refined = self._refine(tf, seeds, dt)
+ if refined is not None:
+ ds, conf = refined
+ elif direct is not None and conf < 0.45:
+ ds, conf = direct, max(conf, 0.3)
+
+ if self.prev_az is not None and self.prev_az.size == az.size:
+ yaw_bins, _ = _centred_shift(self.prev_az, az, max_shift=40)
+ yaw = yaw_bins * float(tf.layout.az_step_deg)
+
+ ds = self._filter_speed(ds, conf, dt)
+
+ self.prev_profile = prof
+ self.prev_az = az
+ self.prev_stamp = stamp
+ self.prev_r = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
+ self.prev_pts = _sample_points(tf, self.n_sample)
+ self.prev_ds = ds if self.prev_ds is None else 0.6 * ds + 0.4 * self.prev_ds
+
+ return EgoMotion(ds=ds, speed=ds / dt, yaw_deg=yaw, conf=float(conf), dt=dt)
+
+ def _filter_speed(self, ds: float, conf: float, dt: float) -> float:
+ """Сгладить оценку скорости с учётом физики состава.
+
+ Сопоставление кадров иногда «срывается» на резкой смене обстановки —
+ например на переходе круглого тоннеля в двухпутный — и выдаёт то ноль,
+ то предел диапазона поиска. Поезд так не умеет: за 0.1 с скорость не
+ меняется больше чем на a·dt. Измерение принимается с весом, равным
+ согласию перепроекции, и ограничивается физическим пределом ускорения,
+ поэтому редкий срыв сглаживается, а настоящее торможение отслеживается
+ за десятые доли секунды.
+ """
+ self.n_updates += 1
+ v_meas = ds / max(dt, 1e-3)
+ if self.v_filt is None or self.n_updates <= WARMUP_FRAMES:
+ self.v_filt = v_meas
+ return ds
+ gain = float(np.clip(conf, 0.05, 0.6))
+ v = self.v_filt + gain * (v_meas - self.v_filt)
+ limit = MAX_ACCEL * dt
+ v = float(np.clip(v, self.v_filt - limit, self.v_filt + limit))
+ self.v_filt = max(v, 0.0)
+ return self.v_filt * dt
+
+ def _refine(self, tf, seeds: list[float], dt: float):
+ """Двухэтапный перебор сдвига вокруг стартовых гипотез."""
+ pts = getattr(self, "prev_pts", None)
+ if pts is None or pts.shape[0] < 500:
+ return None
+ if self.indexer is None or self.indexer.n_az != tf.layout.n_az:
+ self.indexer = RayIndexer(tf.layout)
+
+ hi = MAX_SPEED * dt
+ # гипотезы от дешёвых оценок плюс редкая сетка на весь диапазон;
+ # округление до 10 см убирает дубликаты и держит число проб низким
+ grid = {round(float(np.clip(s, 0.0, hi)), 1) for s in seeds}
+ for s in list(grid):
+ grid.update(round(float(np.clip(s + o, 0.0, hi)), 1) for o in (-0.5, 0.5))
+ grid.update(round(float(x), 1) for x in np.linspace(0.0, hi, 8))
+ cand = np.array(sorted(grid))
+
+ sc = np.array([advance_score(pts, tf.r, tf.valid, self.indexer, s) for s in cand])
+ best = float(cand[int(np.argmax(sc))])
+
+ fine = np.clip(best + np.linspace(-0.2, 0.2, 5), 0.0, hi)
+ sf = np.array([advance_score(pts, tf.r, tf.valid, self.indexer, s) for s in fine])
+ i = int(np.argmax(sf))
+ if sf[i] < 0.05:
+ return None
+ # уточнение параболой по трём точкам вокруг лучшей
+ ds = float(fine[i])
+ if 0 < i < fine.size - 1:
+ y0, y1, y2 = sf[i - 1], sf[i], sf[i + 1]
+ den = y0 - 2 * y1 + y2
+ if abs(den) > 1e-9:
+ ds += 0.5 * (y0 - y2) / den * (fine[1] - fine[0])
+ return float(np.clip(ds, 0.0, hi)), float(sf[i])
+
+ def _direct_advance(self, tf, d_lo: float = 35.0, d_hi: float = 200.0,
+ grad_max: float = 0.6) -> float | None:
+ """Медианное приближение дальних поверхностей, обращённых к сенсору.
+
+ Стены тоннеля идут почти вдоль движения, и их дальность при езде почти
+ не меняется, поэтому они отбрасываются по градиенту дальности вдоль
+ строки: остаются только фронтальные поверхности, для которых убывание
+ дальности равно пройденному пути.
+ """
+ cur = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
+ prev = getattr(self, "prev_r", None)
+ if prev is None or prev.shape != cur.shape:
+ return None
+ with np.errstate(invalid="ignore"):
+ grad = np.abs(np.gradient(cur, axis=1))
+ m = (np.isfinite(cur) & np.isfinite(prev) & (cur > d_lo) & (cur < d_hi)
+ & (grad < grad_max))
+ if m.sum() < 150:
+ return None
+ diff = prev[m] - cur[m]
+ diff = diff[np.abs(diff) < MAX_SPEED * 0.12]
+ if diff.size < 100:
+ return None
+ return float(np.clip(np.median(diff), 0.0, MAX_SPEED * 0.12))
+
+
+def _sample_points(tf, n: int) -> np.ndarray:
+ """Равномерная выборка точек кадра: (N, 3) = (d, u, z) в системе сенсора.
+
+ Берётся именно z сенсора, а не высота над рельсом: перепроекция идёт в
+ решётку лучей, а она задана относительно сенсора.
+ """
+ m = tf.valid & (tf.d > 8.0) & (tf.d < 200.0)
+ idx = np.flatnonzero(m.ravel())
+ if idx.size == 0:
+ return np.zeros((0, 3), np.float32)
+ if idx.size > n:
+ idx = idx[:: max(1, idx.size // n)][:n]
+ return np.stack([tf.d.ravel()[idx], tf.u.ravel()[idx], tf.z.ravel()[idx]],
+ axis=1).astype(np.float32)
+
+
+def _centred_shift(prev: np.ndarray, cur: np.ndarray, max_shift: int) -> tuple[float, float]:
+ """Сдвиг в обе стороны — для рыскания."""
+ a, b = _norm(prev), _norm(cur)
+ n = a.size
+ ks = np.arange(-max_shift, max_shift + 1)
+ sc = np.empty(ks.size, np.float32)
+ for i, k in enumerate(ks):
+ if k >= 0:
+ m = n - k
+ sc[i] = float(np.dot(b[:m], a[k:k + m]))
+ else:
+ m = n + k
+ sc[i] = float(np.dot(b[-k:-k + m], a[:m]))
+ i = int(np.argmax(sc))
+ return float(ks[i]), float(sc[i])
+
+
+# --------------------------------------------------------------------------- T4/T5
+
+class EmdBank:
+ """Коррелятор Хассенштайна–Райхардта на четыре направления.
+
+ Работает на прорежённой решётке: широкопольным клеткам мухи тоже не нужна
+ полная разрешающая способность фасеток, им важна статистика по полю.
+ """
+
+ DIRECTIONS = ((0, 1), (0, -1), (1, 0), (-1, 0)) # (Δкольцо, Δстолбец)
+
+ def __init__(self, decimate: tuple[int, int] = (2, 4), tau_frames: float = 1.5):
+ self.dec = decimate
+ self.alpha = float(np.exp(-1.0 / max(tau_frames, 1e-3)))
+ self.delayed: np.ndarray | None = None
+
+ def _down(self, a: np.ndarray) -> np.ndarray:
+ dh, dw = self.dec
+ h = a.shape[0] // dh * dh
+ w = a.shape[1] // dw * dw
+ return a[:h, :w].reshape(h // dh, dh, w // dw, dw).mean(axis=(1, 3))
+
+ def update(self, signal: np.ndarray) -> np.ndarray:
+ """Вернуть (4, h, w) откликов на движение в четырёх направлениях."""
+ s = self._down(signal).astype(np.float32)
+ if self.delayed is None or self.delayed.shape != s.shape:
+ self.delayed = s.copy()
+ return np.zeros((4,) + s.shape, np.float32)
+
+ d = self.delayed
+ out = np.zeros((4,) + s.shape, np.float32)
+ for k, (di, dj) in enumerate(self.DIRECTIONS):
+ a = np.roll(s, (di, dj), axis=(0, 1))
+ ad = np.roll(d, (di, dj), axis=(0, 1))
+ out[k] = d * a - s * ad # задержанный × соседний, антисимметрично
+ self.delayed = self.alpha * d + (1.0 - self.alpha) * s
+ return out
+
+
+def looming(emd: np.ndarray) -> np.ndarray:
+ """LPLC2: отклик на поток, расходящийся из центра рецептивного поля.
+
+ Дендриты LPLC2 разложены по четырём слоям так, что каждый слой принимает
+ T4/T5 «своего» направления с той стороны поля, куда поток должен уходить при
+ надвигании. Сумма четырёх слоёв и есть дивергенция потока.
+ """
+ right, left, down, up = emd
+ div = np.zeros_like(right)
+ div[:, 1:-1] += right[:, 2:] - left[:, :-2]
+ div[1:-1, :] += down[2:, :] - up[:-2, :]
+ return np.maximum(div, 0.0)
diff --git a/ros2_ws/src/flyguard/flyguard/mushroom_body.py b/ros2_ws/src/flyguard/flyguard/mushroom_body.py
new file mode 100644
index 0000000..43feebe
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/mushroom_body.py
@@ -0,0 +1,524 @@
+"""MUSHROOM BODY — память нормального тоннеля и детектор новизны.
+
+Схема взята из коннектома напрямую и почти не изменена:
+
+* **PN → KC.** Около 2000 клеток Кеньона на полушарие; каждая получает вход
+ всего от ~6 проекционных нейронов, выбранных почти случайно (в FlyWire и
+ hemibrain среднее число «когтей» ≈ 5–7). Это случайная разрежённая проекция,
+ поднимающая размерность примерно в 40 раз.
+* **APL.** Один гигантский тормозный нейрон собирает активность всех KC и
+ возвращает торможение всем сразу. Результат — «победитель забирает всё»:
+ одновременно активны около 5 % клеток Кеньона.
+* **KC → MBON.** Выход считывается немногими выходными нейронами. Синапсы
+ KC→MBON **депрессируются** при повторном предъявлении стимула, поэтому
+ знакомый запах даёт слабый ответ, а новый — сильный. Именно так устроена
+ «familiarity suppression» у MBON-α′3.
+
+Почему это точно наша задача. Разметки нет, и почти все данные — пустой тоннель.
+Геометрический слой честно сообщает обо всём, что торчит в габарит, и вместе с
+препятствиями выдаёт кабельные лотки, ниши, гермозатворы, кромки платформ и
+стрелочные приводы. Грибовидное тело обучается на пустых проездах **без единой
+метки** и после этого молча гасит всё, что тоннель уже показывал, оставляя
+громкими только незнакомые формы.
+
+Хранится не образ, а битовый код: поиск знакомости — это индексация массива,
+поэтому стадия стоит микросекунды и легко укладывается в реальное время.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+
+from .lobula import Candidate
+
+# имена признаков-«проекционных нейронов»; порядок фиксирован и входит в артефакт
+FEATURES = (
+ "log_d", "lat", "abs_lat", "h", "log_w", "log_h", "aspect",
+ "log_rays", "fill", "ang_w", "ang_h", "log_gap", "shadow",
+ "inten", "el", "floor_def", "rings", "cols",
+ "log_depth", "elongation", "containment", "h_min",
+)
+
+
+def describe(c: Candidate) -> np.ndarray:
+ """Вектор признаков кандидата — активность «проекционных нейронов».
+
+ Сознательно смешаны две группы: форма и угловой размер (обобщаются на любой
+ тоннель) и положение в сечении (именно оно делает лоток лотком). Первая даёт
+ переносимость, вторая — способность запомнить штатную обстановку.
+ """
+ d = max(c.d, 1.0)
+ w = max(c.width, 0.02)
+ hh = max(c.height, 0.02)
+ area = max((c.bbox[1] - c.bbox[0] + 1) * (c.bbox[3] - c.bbox[2] + 1), 1)
+ return np.array([
+ np.log(d),
+ c.u,
+ abs(c.u),
+ c.h,
+ np.log(w),
+ np.log(hh),
+ np.log(w / hh),
+ np.log(max(c.n_rays, 1)),
+ c.n_rays / area,
+ np.degrees(w / d),
+ np.degrees(hh / d),
+ np.log1p(max(c.gap, 0.0)),
+ c.shadow,
+ np.log1p(max(c.inten, 0.0)),
+ c.el_deg,
+ np.log1p(max(c.floor_deficit, 0.0)),
+ np.log(max(c.n_rings, 1)),
+ np.log(max(c.n_cols, 1)),
+ np.log1p(max(c.depth, 0.0)),
+ np.log(max(c.depth, 0.05) / max(w, hh)), # вытянутость вдоль пути
+ c.containment,
+ c.h_min, # есть ли опора снизу
+ ], dtype=np.float32)
+
+
+@dataclass
+class MushroomBodyConfig:
+ """Параметры памяти.
+
+ `claws = 6` взято прямо из коннектома: клетка Кеньона получает в среднем
+ 5–7 входов от проекционных нейронов. Это значение и оказалось лучшим в
+ подборе (`tools/tune_memory.py`): 4 когтя дают AUC 0.76, 8 — 0.89, 10 —
+ 0.87, шесть — 0.89…0.90.
+
+ Число клеток Кеньона, напротив, увеличено против мушиных ~2000. Причина
+ инженерная: муха за жизнь встречает сотни запахов, а нам нужно запомнить
+ десятки тысяч видов тоннельной обстановки. При 2000 клетках и 5 % активных
+ память насыщается после нескольких тысяч примеров — подавлены все синапсы, и
+ новым не выглядит уже ничто, включая настоящее препятствие. Разрежённость
+ поэтому тоже снижена: активны 50 клеток из 50 000.
+ """
+
+ n_kc: int = 50_000 # клеток Кеньона
+ claws: int = 6 # входов на клетку Кеньона (из коннектома)
+ sparsity: float = 0.001 # доля активных после торможения APL
+ seed: int = 20240617
+
+
+class MushroomBody:
+ """Разрежённое кодирование + выученная знакомость."""
+
+ def __init__(self, cfg: MushroomBodyConfig | None = None, n_pn: int | None = None):
+ self.cfg = cfg or MushroomBodyConfig()
+ self.n_pn = n_pn or len(FEATURES)
+ self.n_active = max(1, int(round(self.cfg.n_kc * self.cfg.sparsity)))
+ rng = np.random.default_rng(self.cfg.seed)
+
+ # разрежённая случайная проекция PN → KC: по `claws` входов на клетку
+ idx = np.stack([rng.choice(self.n_pn, size=self.cfg.claws, replace=False)
+ for _ in range(self.cfg.n_kc)])
+ sign = rng.choice((-1.0, 1.0), size=idx.shape)
+ w = np.zeros((self.cfg.n_kc, self.n_pn), np.float32)
+ np.put_along_axis(w, idx, sign.astype(np.float32), axis=1)
+ self.W = w
+
+ # нормировка входа (аналог дивизивной нормировки на общую интенсивность)
+ self.mean = np.zeros(self.n_pn, np.float32)
+ self.scale = np.ones(self.n_pn, np.float32)
+
+ # вес синапсов KC → MBON: 1 — «не видели», 0 — «совсем привычно»
+ self.w_mbon = np.ones(self.cfg.n_kc, np.float32)
+ self.n_seen = 0
+
+ # ------------------------------------------------------------------ кодирование
+
+ def fit_normalizer(self, X: np.ndarray) -> None:
+ self.mean = X.mean(0).astype(np.float32)
+ s = X.std(0).astype(np.float32)
+ self.scale = np.where(s > 1e-6, s, 1.0).astype(np.float32)
+
+ def encode(self, X: np.ndarray, device: str | None = None) -> np.ndarray:
+ """Вектора признаков → индексы активных клеток Кеньона, (N, n_active).
+
+ `device="cuda"` переносит проекцию и отбор на GPU: обучение на сотнях
+ тысяч примеров так идёт в десятки раз быстрее. В инференсе кандидатов
+ единицы, и путь через CPU заведомо дешевле запуска ядра.
+ """
+ X = np.atleast_2d(np.asarray(X, np.float32))
+ if X.shape[1] != self.n_pn:
+ raise ValueError(
+ f"память обучена на {self.n_pn} признаках, а дескриптор даёт "
+ f"{X.shape[1]}: набор признаков менялся — переобучите память "
+ f"(tools/train_mushroom_body.py)")
+ k = self.n_active
+ # матрица откликов (N × n_kc) при большой выборке не помещается в память
+ # целиком, поэтому кодирование идёт порциями фиксированного объёма
+ chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
+
+ target_dev = device
+ if target_dev == "auto":
+ from .device import get_device
+ target_dev = get_device("auto")
+
+ if target_dev and target_dev != "cpu":
+ try:
+ import torch
+ out = np.empty((X.shape[0], k), np.int64)
+ with torch.no_grad():
+ m = torch.as_tensor(self.mean, device=target_dev)
+ s = torch.as_tensor(self.scale, device=target_dev)
+ w = torch.as_tensor(self.W, device=target_dev).T.contiguous()
+ for i in range(0, X.shape[0], chunk):
+ t = torch.as_tensor(X[i:i + chunk], device=target_dev)
+ y = ((t - m) / s) @ w
+ out[i:i + chunk] = torch.topk(y, k, dim=1).indices.cpu().numpy()
+ return out
+ except Exception as e:
+ from .device import notify_cuda_error
+ notify_cuda_error(e)
+ # Fallback на CPU ниже
+
+ if X.shape[0] <= chunk:
+ z = (X - self.mean) / self.scale
+ y = z @ self.W.T # (N, n_kc)
+ # APL: глобальное торможение оставляет только сильнейшие
+ return np.argpartition(-y, k - 1, axis=1)[:, :k]
+
+ out = np.empty((X.shape[0], k), np.int64)
+ for i in range(0, X.shape[0], chunk):
+ z = (X[i:i + chunk] - self.mean) / self.scale
+ y = z @ self.W.T
+ out[i:i + chunk] = np.argpartition(-y, k - 1, axis=1)[:, :k]
+ return out
+
+ # ------------------------------------------------------------------ обучение
+
+ def auto_rate(self, n_samples: int, target: float = 1.5) -> float:
+ """Темп депрессии, согласованный с размером обучающей выборки.
+
+ Каждый пример депрессирует `n_active` клеток, поэтому на одну клетку
+ Кеньона в среднем приходится `n · n_active / n_kc` попаданий. Если темп
+ не уменьшать вместе с ростом выборки, после нескольких десятков тысяч
+ примеров подавлены все синапсы и новым не выглядит уже ничто.
+
+ Темп выбирается так, чтобы «типичная» клетка ослабла примерно в e^target
+ раз. Тогда шкала новизны отражает **частоту** обстановки: постоянно
+ встречающиеся лотки и ниши уходят в ноль, а редкая форма остаётся
+ заметной. Это и есть смысл familiarity suppression, а не «видел хоть раз».
+ """
+ mean_hits = max(n_samples * self.n_active / self.cfg.n_kc, 1e-6)
+ return float(np.clip(target / mean_hits, 1e-4, 0.5))
+
+ def learn(self, X: np.ndarray, rate: float = 0.35, device: str | None = None) -> None:
+ """Депрессия синапсов KC→MBON на предъявленных примерах.
+
+ Каждое предъявление умножает вес активных синапсов на (1 − rate), что
+ для набора сразу эквивалентно возведению в степень по числу попаданий, —
+ поэтому цикл по примерам не нужен.
+ """
+ target_dev = device or "cpu" # без явной просьбы — процессор, как раньше
+ if target_dev == "auto":
+ from .device import get_device
+ target_dev = get_device("auto")
+
+ if target_dev.startswith("cuda"):
+ try:
+ import torch
+ dev = torch.device(target_dev)
+ k = self.n_active
+ chunk = max(1, int(2 ** 26 // max(self.cfg.n_kc, 1)))
+ m = torch.as_tensor(self.mean, device=dev)
+ s = torch.as_tensor(self.scale, device=dev)
+ w = torch.as_tensor(self.W, device=dev).T.contiguous()
+ act_list = []
+ with torch.no_grad():
+ for i in range(0, X.shape[0], chunk):
+ t = torch.as_tensor(X[i:i + chunk], device=dev)
+ y = ((t - m) / s) @ w
+ top_idx = torch.topk(y, k, dim=1).indices
+ act_list.append(top_idx)
+ act_t = torch.cat(act_list, dim=0) if len(act_list) > 1 else act_list[0]
+ cnt_t = torch.bincount(act_t.reshape(-1), minlength=self.cfg.n_kc)
+ w_mbon_t = torch.as_tensor(self.w_mbon, device=dev)
+ w_mbon_t *= torch.pow(torch.tensor(1.0 - rate, device=dev), cnt_t.float())
+ self.w_mbon = w_mbon_t.cpu().numpy()
+ self.n_seen += X.shape[0]
+ return
+ except Exception as e:
+ from .device import notify_cuda_error
+ notify_cuda_error(e)
+
+ act = self.encode(X, device="cpu")
+ cnt = np.bincount(act.ravel(), minlength=self.cfg.n_kc)
+ self.w_mbon *= np.power(1.0 - rate, cnt).astype(np.float32)
+ self.n_seen += len(act)
+
+ def novelty(self, X: np.ndarray) -> np.ndarray:
+ """Ответ MBON: 1 — совершенно ново, 0 — вполне привычно."""
+ act = self.encode(X)
+ return self.w_mbon[act].mean(axis=1)
+
+ def novelty_of(self, c: Candidate) -> float:
+ return float(self.novelty(describe(c)[None, :])[0])
+
+ def annotate(self, cands: list[Candidate]) -> list[Candidate]:
+ if not cands:
+ return cands
+ X = np.stack([describe(c) for c in cands])
+ nov = self.novelty(X)
+ for c, n in zip(cands, nov):
+ c.novelty = float(n)
+ return cands
+
+ # ------------------------------------------------------------------ хранение
+
+ def save(self, path: str | Path) -> None:
+ np.savez_compressed(path, w_mbon=self.w_mbon, mean=self.mean, scale=self.scale,
+ n_kc=self.cfg.n_kc, claws=self.cfg.claws,
+ sparsity=self.cfg.sparsity, seed=self.cfg.seed,
+ n_pn=self.n_pn, n_seen=self.n_seen,
+ features=np.array(FEATURES))
+
+ @staticmethod
+ def load(path: str | Path) -> "MushroomBody":
+ d = np.load(path, allow_pickle=False)
+ cfg = MushroomBodyConfig(n_kc=int(d["n_kc"]), claws=int(d["claws"]),
+ sparsity=float(d["sparsity"]), seed=int(d["seed"]))
+ mb = MushroomBody(cfg, n_pn=int(d["n_pn"]))
+ mb.w_mbon = d["w_mbon"].astype(np.float32)
+ mb.mean = d["mean"].astype(np.float32)
+ mb.scale = d["scale"].astype(np.float32)
+ mb.n_seen = int(d["n_seen"])
+ return mb
+
+
+# Признаки для привыкания. Отличие от `FEATURES` принципиальное: здесь нет
+# ничего, что меняется при подъезде к одному и тому же предмету. Ни дальности,
+# ни числа лучей, ни углового размера, ни интенсивности (она падает как 1/R²),
+# ни контраста в метрах (он растёт с дальностью). Остаются физические свойства
+# формы: где она стоит в сечении, какая она и есть ли под ней опора.
+#
+# Иначе механизм не работает вовсе: код одного и того же кронштейна, снятого с
+# 90 и с 40 м, разъезжается, повторы не узнаются, а подъезжающий предмет,
+# наоборот, каждый кадр выглядит новой формой и привыкает сам к себе. Замерено:
+# на полном наборе признаков за двенадцать разных мест новизна падала с 1.000
+# до 0.981 — то есть ни на что.
+#
+# У мухи ту же роль играет инвариантность к концентрации запаха: APL нормирует
+# общий уровень возбуждения, и один и тот же запах слабее или сильнее даёт
+# почти один и тот же набор активных клеток Кеньона.
+HAB_FEATURES = ("lat", "abs_lat", "h", "h_min", "log_w", "log_h",
+ "aspect", "log_depth", "elongation", "containment")
+
+
+def describe_shape(c: Candidate) -> np.ndarray:
+ """Вектор формы, не зависящий от дальности до неё."""
+ w = max(c.width, 0.02)
+ hh = max(c.height, 0.02)
+ return np.array([
+ c.u,
+ abs(c.u),
+ c.h,
+ c.h_min,
+ np.log(w),
+ np.log(hh),
+ np.log(w / hh),
+ np.log1p(max(c.depth, 0.0)),
+ np.log(max(c.depth, 0.05) / max(w, hh)),
+ c.containment,
+ ], dtype=np.float32)
+
+
+@dataclass
+class HabituationConfig:
+ """Параметры привыкания внутри одного проезда.
+
+ `n_kc` намеренно маленькое — 4000 против 50 000 у долговременной памяти, то
+ есть примерно как у самой мухи. Задача здесь обратная: не различить десятки
+ тысяч видов обстановки, а **обобщить** — чтобы седьмой кронштейн гасился
+ первыми шестью, хотя он и не в точности такой же. Чем меньше популяция и
+ чем шире разрежённость, тем сильнее перекрываются коды похожих форм.
+ """
+
+ n_kc: int = 4000
+ claws: int = 4
+ sparsity: float = 0.02 # 80 активных клеток
+ seed: int = 20260214
+ rate: float = 0.25 # депрессия за одно НОВОЕ место
+ place_m: float = 5.0 # насколько разнести места, чтобы считать их разными
+ same_frac: float = 0.30 # доля клеток «с этого же места» — значит, то же самое
+ read_q: float = 0.75 # квантиль отсчёта MBON: доля клеток, которых мало
+ quant: float = 0.5 # шаг огрубления признаков, в долях разброса
+ recover_m: float = 800.0 # путь, за который привыкание сходит наполовину
+ warmup: int = 60 # кандидатов на разогрев нормировки
+ norm_n: int = 300 # после стольких нормировка замирает
+ min_novelty: float = 0.05 # ниже этого привыкание не опускает
+
+
+class Habituation:
+ """Кратковременное привыкание: гасит то, что повторяется в РАЗНЫХ местах пути.
+
+ Обученная память решает задачу «этот тоннель я уже видел». На новом участке
+ она бесполезна по определению: новизна у всего максимальна, улика копится
+ беспрепятственно, и ложных тревог становится 24.3 на километр вместо 9.1.
+ А приватный тест — это как раз новый участок.
+
+ Но даже на совершенно незнакомой линии есть признак, не требующий ни единого
+ прошлого проезда. **Тоннельная обстановка повторяется вдоль пути, а
+ посторонний предмет — нет.** Кабельный кронштейн, рама крепи, стык тюбингов
+ встречаются каждые несколько метров в одном и том же виде; упавший предмет
+ лежит в одном месте. Поэтому привыкание считается не по времени и не по
+ числу кадров, а по **числу разных точек пути**, где встретилась эта форма.
+
+ Разница принципиальна. Настоящий предмет виден сто кадров подряд, но всё это
+ время он стоит в одной точке мира: его код депрессируется один раз и
+ остаётся новым до конца подъезда. Кронштейны за те же сто кадров дают
+ десятки разных точек и гаснут.
+
+ Биологически это та же схема, только с другим учителем. Синапсы KC→MBON
+ депрессируются при повторном предъявлении — это и есть familiarity
+ suppression MBON-α′3. Момент депрессии здесь разрешает координата пути из
+ центрального комплекса: у мухи дофаминергические PPL1/PAM точно так же
+ решают, когда пластичности быть, и их вход зависит от состояния животного,
+ а не только от стимула.
+
+ Привыкание постепенно сходит (`recover_m`): выехав с однообразного перегона
+ на станцию, система снова смотрит в оба.
+ """
+
+ def __init__(self, cfg: HabituationConfig | None = None):
+ self.cfg = cfg or HabituationConfig()
+ self.enc = MushroomBody(
+ MushroomBodyConfig(n_kc=self.cfg.n_kc, claws=self.cfg.claws,
+ sparsity=self.cfg.sparsity, seed=self.cfg.seed),
+ n_pn=len(HAB_FEATURES))
+ n = self.cfg.n_kc
+ self.w = np.ones(n, np.float32)
+ # где в последний раз эта клетка Кеньона участвовала в наблюдении
+ self.last_s = np.full(n, -1e9, np.float32)
+ self.places = 0
+ self._n = 0
+ self._sum = np.zeros(len(HAB_FEATURES), np.float64)
+ self._sq = np.zeros(len(HAB_FEATURES), np.float64)
+
+ @property
+ def level(self) -> float:
+ """Насколько привыкла популяция: 0 — всё ново, 1 — всё знакомо."""
+ return float(1.0 - self.w.mean())
+
+ # ------------------------------------------------------------------ такт
+
+ def advance(self, ds: float) -> None:
+ """Забывание по пройденному пути."""
+ if ds <= 0 or self.cfg.recover_m <= 0:
+ return
+ k = 1.0 - 0.5 ** (ds / self.cfg.recover_m)
+ self.w += (1.0 - self.w) * k
+
+ def _fit(self, X: np.ndarray) -> None:
+ """Дивизивная нормировка по накопленной статистике проезда.
+
+ После `norm_n` кандидатов замирает, и это не мелочь. Пока нормировка
+ плывёт, вместе с ней плывёт и код: у неподвижного предмета, чьи признаки
+ формы вообще не меняются, набор активных клеток обновлялся настолько,
+ что предмет засчитывался как двадцать шесть разных мест и гасил сам
+ себя. С замороженной нормировкой одна и та же форма даёт один и тот же
+ код, и правило «то же место — не повтор» наконец работает.
+
+ Порог низкий намеренно. При 4000 в бэге `roundT_doubleT` (всего около
+ 570 кандидатов за проезд) нормировка не замирала вовсе: привыкание
+ осталось на 0.20, медианная новизна кандидата 1.00 и ложных треков
+ ровно столько же, сколько без привыкания. Трёхсот кандидатов на десять
+ признаков для оценки среднего и разброса достаточно с запасом.
+ """
+ self._n += X.shape[0]
+ if self._n > self.cfg.norm_n:
+ return
+ self._sum += X.sum(0)
+ self._sq += (X.astype(np.float64) ** 2).sum(0)
+ m = self._sum / self._n
+ v = np.maximum(self._sq / self._n - m * m, 1e-12)
+ self.enc.mean = m.astype(np.float32)
+ self.enc.scale = np.sqrt(v).astype(np.float32)
+
+ def _coarse(self, X: np.ndarray) -> np.ndarray:
+ """Огрубить признаки до шага `quant` разброса — до кодирования.
+
+ Без этого механизм не работает вовсе. Два кронштейна, снятые в разных
+ местах, отличаются на сантиметры, случайная проекция разносит их в
+ разные наборы клеток, и второй не узнаёт первого. Замер: при ёмкости,
+ достаточной чтобы не насыщаться (20 000 клеток), привыкание давало
+ ровно ноль — 7.5 ложных трека на км против тех же 7.5 без него. Весь
+ видимый эффект маленькой популяции был не узнаванием повторов, а
+ насыщением, которое одинаково давит и обстановку, и предмет.
+
+ Огрубление до половины разброса заставляет близкие формы давать
+ буквально один и тот же код. Это coarse coding, и у мухи оно тоже есть:
+ клетка Кеньона реагирует на широкую область пространства запахов, а не
+ на точку.
+ """
+ q = self.cfg.quant
+ if q <= 0:
+ return X
+ z = (X - self.enc.mean) / self.enc.scale
+ return (np.round(z / q) * q) * self.enc.scale + self.enc.mean
+
+ def update(self, cands: list[Candidate], s_path: float, ds: float) -> None:
+ """Приглушить знакомое и запомнить то, что встретилось в новом месте.
+
+ `s_path` — пройденный путь, м; положение формы в мире считается как
+ `s_path + c.d`, поэтому неподвижный предмет остаётся в одной точке,
+ сколько бы кадров к нему ни ехали. Оценка собственного движения копит
+ около 3 % ошибки, то есть за подъезд с сотни метров точка уползает на
+ два-три метра — отсюда и `place_m`: меньший разнос считается тем же
+ местом, больший — новым.
+ """
+ self.advance(ds)
+ if not cands:
+ return
+ X = np.stack([describe_shape(c) for c in cands])
+ self._fit(X)
+ if self._n < self.cfg.warmup:
+ return # нормировка ещё не осмысленна
+ act = self.enc.encode(self._coarse(X))
+ # Отсчёт берётся КВАНТИЛЕМ, а не средним, и это решает главную проблему
+ # механизма. Коды похожих форм перекрываются частично: кронштейн, гасясь
+ # сам, задевает часть клеток предмета. При среднем половина подавленных
+ # клеток уже вдвое снижает новизну предмета — замерено, что новизна
+ # вставленного человека падала с 0.76 до 0.20, а обнаружение на 70–90 м
+ # со 100 % до 58 %. Квантиль 0.75 требует, чтобы подавлено было не менее
+ # трёх четвертей клеток кода: частичное перекрытие форму уже не гасит,
+ # а настоящий повтор гасит полностью.
+ fam = np.quantile(self.w[act], self.cfg.read_q, axis=1)
+
+ lo = self.cfg.min_novelty
+ for c, f in zip(cands, fam):
+ # Берётся минимум, а не произведение: два канала знакомости, и
+ # достаточно одного, чтобы приглушить. Перемножение давило бы
+ # предмет дважды за одно и то же.
+ c.novelty = float(max(min(c.novelty, f), lo))
+
+ keep = 1.0 - self.cfg.rate
+ for i, c in enumerate(cands):
+ s_obj = np.float32(s_path + c.d)
+ a = act[i]
+ same = np.abs(self.last_s[a] - s_obj) <= self.cfg.place_m
+ # Отметка ставится всегда, в том числе клеткам, впервые попавшим в
+ # код: иначе подъезжающий предмет каждый кадр вербует свежие клетки
+ # и депрессирует сам себя.
+ self.last_s[a] = s_obj
+ if same.mean() >= self.cfg.same_frac:
+ continue # то же самое место — это не повтор
+ self.w[a] *= keep
+ self.places += 1
+
+
+class NoMemory:
+ """Заглушка: пока память не обучена, всё считается новым."""
+
+ n_seen = 0
+
+ @staticmethod
+ def annotate(cands: list[Candidate]) -> list[Candidate]:
+ return cands
+
+ @staticmethod
+ def novelty_of(_c: Candidate) -> float:
+ return 1.0
diff --git a/ros2_ws/src/flyguard/flyguard/node.py b/ros2_ws/src/flyguard/flyguard/node.py
new file mode 100644
index 0000000..de0c797
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/node.py
@@ -0,0 +1,708 @@
+"""ROS 2-нода FlyGuard.
+
+Подписывается на облако точек лидара, прогоняет конвейер и публикует:
+
+ /flyguard/obstacle flyguard_msgs/ObstacleStatus — главный программный выход
+ /flyguard/markers visualization_msgs/MarkerArray — рамки объектов и габарит
+ /flyguard/view_cloud sensor_msgs/PointCloud2 — облако обзора для RViz
+ /flyguard/debug_cloud sensor_msgs/PointCloud2 — раскраска по новизне
+ /flyguard/brain sensor_msgs/Image — схема мозга мухи с активностью
+ /flyguard/diagnostics diagnostic_msgs/DiagnosticArray — задержки по стадиям
+
+Обработка идёт в отдельном потоке, и из очереди всегда берётся **последний**
+пришедший кадр: система реального времени обязана отвечать на текущую обстановку,
+а не доедать накопившееся прошлое.
+"""
+from __future__ import annotations
+
+import array
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+from geometry_msgs.msg import Point, TransformStamped
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Bool, Float32, Header
+from tf2_ros import StaticTransformBroadcaster
+from visualization_msgs.msg import Marker, MarkerArray
+
+from flyguard_msgs.msg import DetectedObject, ObstacleStatus
+
+from . import ros_conv
+from .cdr import parse_pointcloud2
+from .export import gauge_outline
+from .mushroom_body import MushroomBody
+from .pipeline import FlyGuard, Params
+
+
+class FlyGuardNode(Node):
+ def __init__(self):
+ super().__init__("flyguard")
+ p = self.declare_parameters("", [
+ ("input_topic", "/lidar_points"),
+ ("fallback_topics", ["/sensing/lidar/hesai128/pointcloud", "/points_raw"]),
+ ("frame_id", ""),
+ ("best_effort", True),
+ ("raw_subscription", True),
+ ("queue_depth", 20),
+ ("async_worker", False),
+ ("memory_path", ""),
+ ("mbon_path", ""),
+ ("enable_mbon", True),
+ # плотные стадии (сетчатка, ламина, кластеризация): auto — видеокарта,
+ # если PyTorch её видит, иначе процессор; cuda; cpu
+ ("device", "auto"),
+ ("mbon_power", 1.5),
+ ("mbon_blend", 1.0),
+ ("fov_deg", 30.0),
+ ("half_width", 1.2),
+ ("h_lo", 0.28),
+ ("h_hi", 2.3),
+ ("h_top", 3.3),
+ ("half_width_top", 1.0),
+ ("top_d_max", 90.0),
+ ("h_lo_core", 0.16),
+ ("core_from", 30.0),
+ ("ctx_up", 4.0),
+ ("split_adv", 0.0),
+ ("split_gap", 6.0),
+ ("split_near", 55.0),
+ ("split_top", 1),
+ ("enable_accumulator", True),
+ ("acc_near", 55.0),
+ ("acc_gain", 1.5),
+ ("enable_habituation", False),
+ ("hab_rate", 0.25),
+ ("hab_place_m", 5.0),
+ ("hab_recover_m", 800.0),
+ ("d_min", 4.0),
+ ("d_max", 220.0),
+ ("min_rays", 4),
+ ("publish_debug_cloud", False),
+ ("publish_markers", True),
+ ("brain_view", False),
+ # схема | облако нейронов коннектома | гибрид (панели + облако)
+ ("brain_style", "hybrid"),
+ ("brain_period", 0.2),
+ # 1 — 1180 пикселей по ширине; 2–3 — для экрана и видео в 2K/4K
+ ("brain_scale", 1),
+ ])
+ self.par = {q.name: q.value for q in p}
+
+ brain_on = bool(self.par["brain_view"])
+ params = Params(device=str(self.par["device"]),
+ fov_deg=float(self.par["fov_deg"]),
+ half_width=float(self.par["half_width"]),
+ h_lo=float(self.par["h_lo"]), h_hi=float(self.par["h_hi"]),
+ h_top=float(self.par["h_top"]),
+ half_width_top=float(self.par["half_width_top"]),
+ top_d_max=float(self.par["top_d_max"]),
+ h_lo_core=float(self.par["h_lo_core"]),
+ core_from=float(self.par["core_from"]),
+ ctx_up=float(self.par["ctx_up"]),
+ split_adv=float(self.par["split_adv"]),
+ split_gap=float(self.par["split_gap"]),
+ split_near=float(self.par["split_near"]),
+ split_top=int(self.par["split_top"]),
+ enable_accumulator=bool(self.par["enable_accumulator"]),
+ acc_near=float(self.par["acc_near"]),
+ acc_gain=float(self.par["acc_gain"]),
+ enable_habituation=bool(self.par["enable_habituation"]),
+ hab_rate=float(self.par["hab_rate"]),
+ hab_place_m=float(self.par["hab_place_m"]),
+ hab_recover_m=float(self.par["hab_recover_m"]),
+ d_min=float(self.par["d_min"]), d_max=float(self.par["d_max"]),
+ min_rays=int(self.par["min_rays"]),
+ # каналы T4/T5 и LPLC2 считаются только когда есть кому
+ # их показать: на решение они пока не влияют
+ enable_mbon=bool(self.par["enable_mbon"]),
+ mbon_power=float(self.par["mbon_power"]),
+ mbon_blend=float(self.par["mbon_blend"]),
+ enable_looming=brain_on)
+
+ memory = None
+ mem_path = str(self.par["memory_path"])
+ if mem_path and Path(mem_path).exists():
+ memory = MushroomBody.load(mem_path)
+ self.get_logger().info(
+ f"память тоннеля загружена: {mem_path} "
+ f"({memory.cfg.n_kc} клеток Кеньона, обучена на {memory.n_seen} примерах)")
+ else:
+ self.get_logger().warn(
+ "память тоннеля не задана — штатные конструкции тоннеля не подавляются, "
+ "ложных тревог будет заметно больше")
+
+ readout = None
+ mb_path = str(self.par["mbon_path"])
+ if mb_path and Path(mb_path).exists():
+ from .mbon_readout import MbonReadout
+ readout = MbonReadout.load(mb_path)
+ self.get_logger().info(
+ f"считывание MBON загружено: {mb_path} "
+ f"({readout.cfg.n_kc} клеток Кеньона, {readout.n_pn} признаков)")
+ elif bool(self.par["enable_mbon"]):
+ self.get_logger().warn(
+ "считывание MBON не задано — вес улики считается ручной формулой, "
+ "ложных тревог будет больше")
+
+ # Видеокарта поднимается в фоне: подписка не ждёт прогрева ядер CUDA
+ self._t_start = time.monotonic()
+ self.fg = FlyGuard(params, memory=memory, readout=readout, gpu_background=True)
+ self._report_device()
+ self.brain = None
+ self.brain_period = float(self.par["brain_period"])
+ self._brain_last = 0.0
+ if brain_on:
+ style = str(self.par["brain_style"]).lower()
+ if style == "scheme":
+ from .brain_view import BrainView
+ self.brain = BrainView()
+ elif style == "cloud":
+ from .brain_atlas import NeuronCloud
+ self.brain = NeuronCloud(scale=int(self.par["brain_scale"]))
+ else:
+ from .brain_hybrid import BrainHybrid
+ self.brain = BrainHybrid(scale=int(self.par["brain_scale"]))
+ if not getattr(self.brain, "enabled", False):
+ # атласа нет — падать незачем, показываем схему
+ from .brain_view import BrainView
+ self.get_logger().warn(
+ f"вид «{style}» недоступен (нет атласа нейронов), беру схему")
+ self.brain = BrainView()
+
+ qos = QoSProfile(
+ history=QoSHistoryPolicy.KEEP_LAST, depth=int(self.par["queue_depth"]),
+ reliability=(QoSReliabilityPolicy.BEST_EFFORT if self.par["best_effort"]
+ else QoSReliabilityPolicy.RELIABLE),
+ durability=QoSDurabilityPolicy.VOLATILE)
+
+ # Кадр лидара — это 24 МБ, и сборка из них Python-объекта sensor_msgs
+ # стоит дороже всей нашей обработки: на записи с полным круговым сканом
+ # так терялась половина кадров. Поэтому по умолчанию берём сырые байты
+ # CDR и разбираем своим парсером — он строит numpy-вид поверх буфера
+ # без копирования. Обычный путь остаётся под флагом, на случай
+ # нестандартной раскладки полей.
+ self.raw = bool(self.par["raw_subscription"])
+ topics = list(dict.fromkeys([str(self.par["input_topic"])]
+ + list(self.par["fallback_topics"])))
+ self._qos = qos
+ self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, qos,
+ raw=self.raw)
+ for t in topics]
+ self.get_logger().info(
+ f"подписка на: {', '.join(topics)}"
+ f" ({'сырые байты CDR' if self.raw else 'разбор через rclpy'})")
+ # Сторож входа. Издатель на топике есть, а кадров нет — почти всегда это
+ # контейнер без --ipc host и `ros2 bag play` на хосте: Fast DDS шлёт
+ # кадры через /dev/shm, которой у них общей нет, и теряет их молча.
+ # Без подсказки такой запуск выглядит как пустой тоннель.
+ self._topics = topics
+ self._pub_seen_at: float | None = None
+ self._watch = self.create_timer(1.0, self._check_input)
+
+ self.pub_status = self.create_publisher(ObstacleStatus, "/flyguard/obstacle", 10)
+ self.pub_flag = self.create_publisher(Bool, "/flyguard/detected", 10)
+ self.pub_dist = self.create_publisher(Float32, "/flyguard/distance", 10)
+ self.pub_markers = self.create_publisher(MarkerArray, "/flyguard/markers", 5)
+ self.pub_diag = self.create_publisher(DiagnosticArray, "/flyguard/diagnostics", 5)
+ self.pub_cloud = self.create_publisher(PointCloud2, "/flyguard/debug_cloud", 2)
+ self.pub_view = self.create_publisher(PointCloud2, "/flyguard/view_cloud", 2)
+ self.pub_brain = None
+ if self.brain is not None:
+ from sensor_msgs.msg import Image
+ self.pub_brain = self.create_publisher(Image, "/flyguard/brain", 2)
+
+ # Имя кадра лидара в записях различается («lidar_livox», «hesai_lidar»),
+ # поэтому нода публикует свой вывод во всегда одинаковом кадре `lidar`
+ # и отдаёт статическое тождественное преобразование к пришедшему. Тогда
+ # один и тот же конфиг RViz работает с любым бэгом.
+ self.fixed_frame = "lidar"
+ self._tf = StaticTransformBroadcaster(self)
+ self._tf_sent: set[str] = set()
+ self._alarm = False # была ли тревога на прошлом кадре
+ self._alarm_frames = 0
+ self._jumps = 0 # скачки времени записи, о которых уже сказано
+
+ self._latest = None
+ self._lock = threading.Lock()
+ self._wake = threading.Event()
+ self._stop = False
+ self._dropped = 0
+ self._received = 0
+ self._cycle_ms = 0.0
+ self.async_worker = bool(self.par["async_worker"])
+ self._worker = None
+ if self.async_worker:
+ self._worker = threading.Thread(target=self._loop, daemon=True)
+ self._worker.start()
+
+ # ------------------------------------------------------------------ приём
+
+ def _check_input(self) -> None:
+ if self._received:
+ self._watch.cancel()
+ return
+ pubs = sum(self.count_publishers(t) for t in self._topics)
+ if not pubs:
+ self._adopt_foreign_cloud()
+ return
+ now = time.monotonic()
+ if self._pub_seen_at is None:
+ self._pub_seen_at = now
+ elif now - self._pub_seen_at >= 5.0:
+ self.get_logger().error(
+ f"на топике лидара есть издатель, а кадров нет уже "
+ f"{now - self._pub_seen_at:.0f} с. Если bag проигрывается на хосте, "
+ f"запустите контейнер с --ipc host: без него кадры идут через "
+ f"/dev/shm, общей у контейнера с хостом нет", throttle_duration_sec=10.0)
+
+ def _adopt_foreign_cloud(self) -> None:
+ """Облако идёт в топик, которого нет в списке, — подписаться и на него.
+
+ Имя топика у записей разное (в наших двух разное уже), и у контрольной
+ записи может оказаться третье. Узел, молча ждущий не тот топик, выглядит
+ как пустой тоннель. Свои топики `/flyguard/...` не в счёт.
+ """
+ for name, types in self.get_topic_names_and_types():
+ if (name in self._topics or name.startswith("/flyguard/")
+ or "sensor_msgs/msg/PointCloud2" not in types):
+ continue
+ self.get_logger().warn(
+ f"облако точек идёт в {name}, а узел слушал "
+ f"{', '.join(self._topics)} — подписываюсь и на него "
+ f"(явно: input_topic:={name})")
+ self._topics.append(name)
+ self.subs.append(self.create_subscription(
+ PointCloud2, name, self._on_cloud, self._qos, raw=self.raw))
+
+ def _on_cloud(self, msg) -> None:
+ self._received += 1
+ if not self.raw:
+ ok, why = ros_conv.has_required_fields(msg)
+ if not ok:
+ self.get_logger().warn(f"кадр пропущен: {why}", throttle_duration_sec=5.0)
+ return
+ if not self.async_worker:
+ # Обработка прямо в колбэке. Такт конвейера втрое короче периода
+ # кадров, поэтому исполнителю ROS есть когда работать, а отдельный
+ # поток здесь только отнимает GIL у приёма: в измерениях он ронял
+ # выработку с 10 до 2 Гц, хотя сам такт оставался 32 мс.
+ # Отбрасывание устаревших кадров при этом делает очередь DDS:
+ # её глубина `queue_depth` и есть «хранить только свежее».
+ t0 = time.perf_counter()
+ try:
+ self._process(msg)
+ except Exception as exc:
+ self.get_logger().error(f"сбой обработки кадра: {exc!r}")
+ self._cycle_ms = (time.perf_counter() - t0) * 1e3
+ return
+ with self._lock:
+ if self._latest is not None:
+ self._dropped += 1
+ self._latest = msg
+ self._wake.set()
+
+ # ------------------------------------------------------------------ обработка
+
+ def _loop(self) -> None:
+ while not self._stop:
+ self._wake.wait(timeout=0.5)
+ self._wake.clear()
+ with self._lock:
+ msg, self._latest = self._latest, None
+ if msg is None:
+ continue
+ t0 = time.perf_counter()
+ try:
+ self._process(msg)
+ except Exception as exc: # нода не должна падать на кадре
+ self.get_logger().error(f"сбой обработки кадра: {exc!r}")
+ # полный такт рабочего потока: конвейер плюс разбор и публикация
+ self._cycle_ms = (time.perf_counter() - t0) * 1e3
+
+ def _process(self, msg) -> None:
+ if self.raw:
+ pc = parse_pointcloud2(msg)
+ header = Header()
+ sec = int(pc.stamp)
+ header.stamp.sec = sec
+ header.stamp.nanosec = int(round((pc.stamp - sec) * 1e9))
+ header.frame_id = pc.frame_id
+ else:
+ pc = ros_conv.from_ros(msg)
+ header = msg.header
+
+ want_view = self.pub_view.get_subscription_count() > 0
+ need_debug = (bool(self.par["publish_debug_cloud"]) or self.brain is not None
+ or want_view)
+ res = self.fg.process(pc, keep_debug=need_debug)
+ if res is None:
+ self.get_logger().info(
+ f"калибровка решётки лучей: {self.fg.frames_seen}/{self.fg.p.calib_frames}",
+ throttle_duration_sec=2.0)
+ return
+
+ if self.fg.time_jumps != self._jumps:
+ self._jumps = self.fg.time_jumps
+ self._alarm = False
+ self.get_logger().info("время записи скакнуло (перемотка или повтор) — "
+ "треки и одометрия начаты заново")
+
+ src_frame = header.frame_id or self.fixed_frame
+ self._ensure_tf(src_frame)
+ frame = str(self.par["frame_id"]) or self.fixed_frame
+ d = res.decision
+
+ out = ObstacleStatus()
+ out.header = header
+ out.header.frame_id = frame
+ out.detected = bool(d.detected)
+ out.emergency = bool(d.emergency)
+ out.distance = float(d.distance)
+ out.time_to_collision = float(d.ttc)
+ out.confidence = float(d.confidence)
+ out.speed = float(d.speed)
+ out.stopping_distance = float(d.stopping_distance)
+ out.processing_ms = float(res.total_ms)
+ for o in d.objects:
+ m = DetectedObject()
+ m.distance = float(o.distance); m.lateral = float(o.lateral)
+ m.height = float(o.height); m.width = float(o.width)
+ m.size_v = float(o.size_v); m.confidence = float(o.confidence)
+ m.novelty = float(o.novelty); m.n_rays = int(o.n_rays)
+ m.track_id = int(o.track_id); m.time_to_collision = float(o.ttc)
+ out.objects.append(m)
+ self.pub_status.publish(out)
+ self.pub_flag.publish(Bool(data=bool(d.detected)))
+ self.pub_dist.publish(Float32(data=float(d.distance if d.detected else -1.0)))
+ self._report(d)
+
+ if bool(self.par["publish_markers"]):
+ self.pub_markers.publish(self._markers(d, frame, header.stamp, res.corridor))
+ if want_view and res.tf is not None:
+ self.pub_view.publish(self._view_cloud(res, frame, header.stamp))
+ self._publish_diag(res, header.stamp)
+
+ if bool(self.par["publish_debug_cloud"]):
+ cloud = self._debug_cloud(res, frame, header.stamp)
+ if cloud is not None:
+ self.pub_cloud.publish(cloud)
+
+ # схема мозга рисуется реже кадров лидара: она для человека, не для системы
+ if self.brain is not None and self.pub_brain is not None:
+ now = time.monotonic()
+ if now - self._brain_last >= self.brain_period:
+ self._brain_last = now
+ img = self.brain.render(res)
+ if img is not None:
+ self.pub_brain.publish(self.brain.to_msg(img, header.stamp, frame))
+
+ def _report(self, d) -> None:
+ """Итог в консоль: у стенда результат виден без RViz и `ros2 topic echo`.
+
+ Пишется смена состояния, а пока тревога держится — ближайшая дальность
+ не чаще раза в секунду: построчный вывод на 10 Гц читать невозможно.
+ """
+ if d.detected:
+ self._alarm_frames += 1
+ text = (f"{d.distance:.1f} м, уверенность {d.confidence:.2f}, "
+ f"объектов {len(d.objects)}")
+ if np.isfinite(d.ttc) and d.speed > 0.5:
+ text += f", до столкновения {d.ttc:.1f} с"
+ head = "ЭКСТРЕННОЕ ТОРМОЖЕНИЕ" if d.emergency else "ПРЕПЯТСТВИЕ"
+ if not self._alarm:
+ self.get_logger().warn(f"{head}: {text}")
+ else:
+ self.get_logger().warn(f"{head.lower()}: {text}",
+ throttle_duration_sec=1.0)
+ elif self._alarm:
+ self.get_logger().info("путь свободен")
+ self._alarm = bool(d.detected)
+
+ def _debug_cloud(self, res, frame: str, stamp):
+ """Лучи кандидатов, раскрашенные по новизне, — для наглядности в RViz."""
+ tf = res.tf
+ if tf is None or not res.candidates:
+ return None
+ xs, ys, zs, ws = [], [], [], []
+ for c in res.candidates:
+ rays = c.extra.get("rays")
+ if rays is None:
+ continue
+ ii, jj = rays
+ xs.append(tf.u[ii, jj])
+ ys.append(-tf.d[ii, jj])
+ zs.append(tf.h[ii, jj])
+ ws.append(np.full(ii.size, c.novelty, np.float32))
+ if not xs:
+ return None
+ return self._cloud_msg(np.concatenate(xs), np.concatenate(ys),
+ np.concatenate(zs), np.concatenate(ws), frame, stamp)
+
+ def _view_cloud(self, res, frame: str, stamp):
+ """Облако обзора для RViz: сектор обработки в координатах пути.
+
+ Сырое облако — до 900 тысяч точек и 24 МБ на кадр: RViz на нём тормозит,
+ а в части записей оно ещё и идёт в другой топик, которого конфиг RViz не
+ знает. Здесь только лучи сектора обработки (до 77 тысяч), выровненные по
+ плоскости рельсов: x — поперёк пути, −y — вдоль, z — высота над головкой
+ рельса, как у рамок препятствий, так что рамка стоит ровно на полу.
+ Публикуется, только когда на топик кто-то подписан.
+ """
+ tf = res.tf
+ m = tf.valid & (tf.d > 0.5) & (tf.d < 250.0) & np.isfinite(tf.h)
+ return self._cloud_msg(tf.u[m], -tf.d[m], tf.h[m], tf.inten[m], frame, stamp)
+
+ @staticmethod
+ def _cloud_msg(xs, ys, zs, ws, frame: str, stamp) -> PointCloud2:
+ pts = np.empty(xs.size, dtype=np.dtype([("x", "f4"), ("y", "f4"), ("z", "f4"),
+ ("intensity", "f4")]))
+ pts["x"] = xs
+ pts["y"] = ys
+ pts["z"] = zs
+ pts["intensity"] = ws
+
+ msg = PointCloud2()
+ msg.header.stamp = stamp
+ msg.header.frame_id = frame
+ msg.height = 1
+ msg.width = pts.size
+ msg.fields = [PointField(name=n, offset=o, datatype=PointField.FLOAT32, count=1)
+ for n, o in (("x", 0), ("y", 4), ("z", 8), ("intensity", 12))]
+ msg.is_bigendian = False
+ msg.point_step = 16
+ msg.row_step = 16 * pts.size
+ msg.is_dense = True
+ # array('B'), а не bytes: из bytes rclpy проверяет каждый байт на Python —
+ # на облаке обзора это 45 мс на кадр вместо одной (замерено).
+ msg.data = array.array("B", pts.tobytes())
+ return msg
+
+ def _ensure_tf(self, src_frame: str) -> None:
+ if src_frame in self._tf_sent or src_frame == self.fixed_frame:
+ return
+ t = TransformStamped()
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = self.fixed_frame
+ t.child_frame_id = src_frame
+ t.transform.rotation.w = 1.0
+ self._tf.sendTransform(t)
+ self._tf_sent.add(src_frame)
+ self.get_logger().info(f"кадр лидара «{src_frame}» связан с «{self.fixed_frame}»")
+
+ # ------------------------------------------------------------------ визуализация
+
+ def _markers(self, d, frame: str, stamp, corridor=None) -> MarkerArray:
+ arr = MarkerArray()
+ clear = Marker()
+ clear.header.frame_id = frame
+ clear.header.stamp = stamp
+ clear.action = Marker.DELETEALL
+ arr.markers.append(clear)
+
+ for i, o in enumerate(d.objects):
+ m = Marker()
+ m.header.frame_id = frame
+ m.header.stamp = stamp
+ m.ns = "flyguard"
+ m.id = i + 1
+ m.type = Marker.CUBE
+ m.action = Marker.ADD
+ # координаты пути: вперёд = −Y, вправо = +X, вверх = +Z от головки
+ # рельса — те же, что у облака обзора, поэтому рамка стоит на полу.
+ # Поперёк — смещение в системе лидара, а не от оси пути: в кривой
+ # облако не выпрямлено, и рамка по `lateral` встала бы в стороне
+ # от своих точек.
+ x = float(o.sensor_x)
+ m.pose.position.x = x if np.isfinite(x) else float(o.lateral)
+ m.pose.position.y = float(-o.distance)
+ m.pose.position.z = float(o.height + o.size_v / 2)
+ m.pose.orientation.w = 1.0
+ m.scale.x = max(float(o.width), 0.3)
+ m.scale.y = max(float(o.width), 0.3)
+ m.scale.z = max(float(o.size_v), 0.3)
+ hot = float(np.clip(o.confidence, 0.0, 1.0))
+ m.color.r = 1.0
+ m.color.g = float(1.0 - hot)
+ m.color.b = 0.0
+ m.color.a = 0.55
+ arr.markers.append(m)
+
+ txt = Marker()
+ txt.header = m.header
+ txt.ns = "flyguard_text"
+ txt.id = 1000 + i
+ txt.type = Marker.TEXT_VIEW_FACING
+ txt.action = Marker.ADD
+ txt.pose = m.pose
+ txt.pose.position.z += 1.0
+ # 1.4 м: при 0.8 подпись у дальней рамки читалась с трудом
+ txt.scale.z = 1.4
+ txt.color.r = txt.color.g = txt.color.b = txt.color.a = 1.0
+ # латиница: в шрифте RViz нет кириллицы, и «55 м» выходило «55 »
+ txt.text = f"{o.distance:.0f} m p={o.confidence:.2f}"
+ arr.markers.append(txt)
+ arr.markers.extend(self._gauge_markers(frame, stamp, corridor))
+ return arr
+
+ def _gauge_markers(self, frame: str, stamp, corridor=None) -> list[Marker]:
+ """Контур габарита в координатах облака обзора.
+
+ Узел проверяет объединение двух габаритов (`TrackFrame.lateral`):
+ прямого — вдоль оси лидара, и изогнутого — вдоль оценённой оси пути.
+ Облако обзора выровнено по плоскости рельсов, но в кривой не
+ выпрямлено, поэтому в кривой рисуются оба: изогнутый ярко, прямой
+ бледно. На прямом пути они совпадают, и рисуется один. Что внутри
+ оранжевого контура, то узел и проверяет; колонна или шкаф за контуром —
+ не его забота. Рамки поперёк — через 20 м, для глубины.
+ """
+ p = self.fg.p
+ far = min(p.d_max, 200.0)
+ boxes = [(p.half_width, p.h_lo, p.h_hi, p.d_min, far)]
+ if p.h_top > p.h_hi and p.half_width_top > 0.0:
+ boxes.append((p.half_width_top, p.h_hi, p.h_top, p.d_min, min(p.top_d_max, far)))
+ curved = None
+ if corridor is not None and corridor.n_slices > 0:
+ ds = np.arange(p.d_min, far + 1e-6, 5.0, dtype=np.float32)
+ # меньше 15 см контуры сливаются в один — второй незачем
+ if float(np.abs(corridor.centre(ds)).max()) > 0.15:
+ curved = corridor.centre
+ layers = [(None, 0.3 if curved is not None else 0.7)]
+ if curved is not None:
+ layers.append((curved, 0.8))
+ out = []
+ for i, (centre, alpha) in enumerate(layers):
+ m = Marker()
+ m.header.frame_id = frame
+ m.header.stamp = stamp
+ m.ns = "flyguard_gauge"
+ m.id = i
+ m.type = Marker.LINE_LIST
+ m.action = Marker.ADD
+ m.pose.orientation.w = 1.0
+ m.scale.x = 0.04
+ m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.55, 0.0, alpha
+ m.points = [Point(x=float(x), y=float(y), z=float(z))
+ for x, y, z in gauge_outline(boxes, centre)]
+ out.append(m)
+ return out
+
+ def _report_device(self) -> None:
+ """Где идёт счёт — первой строкой журнала: на стенде это видно без RViz."""
+ fg = self.fg
+ if fg.gpu_pending:
+ self.get_logger().info(
+ "вычисления: пока процессор — видеокарту проверяю и прогреваю в фоне "
+ "(в Docker под WSL до 20 с), результат тот же")
+ self._gpu_watch = self.create_timer(0.5, self._watch_gpu)
+ elif fg.gpu_active:
+ self._say_gpu()
+ elif fg.gpu_error:
+ self.get_logger().warn(f"вычисления: процессор — видеокарта не поднялась ({fg.gpu_error})")
+ else:
+ self._say_cpu()
+ self._gpu_on = fg.gpu_active
+
+ def _say_gpu(self, note: str = "") -> None:
+ from .device import get_device_info
+ info = get_device_info(str(self.par["device"]))
+ self.get_logger().info(
+ f"вычисления: видеокарта {info.get('name', '?')} "
+ f"({info.get('total_memory_mb', 0) / 1024:.0f} ГБ, CUDA {info.get('cuda_version')}) — "
+ f"сетчатка, ламина, кластеризация; остальное на процессоре{note}")
+
+ def _say_cpu(self) -> None:
+ why = ("задано device=cpu" if str(self.par["device"]).lower() == "cpu"
+ else "CUDA недоступна: нет видеокарты, драйвера или контейнер запущен без --gpus")
+ self.get_logger().info(f"вычисления: процессор ({why})")
+
+ def _watch_gpu(self) -> None:
+ """Видеокарта поднималась в фоне — сказать в журнал, чем кончилось."""
+ fg = self.fg
+ if fg.gpu_pending:
+ return
+ self._gpu_watch.cancel()
+ self._gpu_on = fg.gpu_active
+ if fg.gpu_active:
+ self._say_gpu(f" (готова через {time.monotonic() - self._t_start:.1f} с после старта)")
+ elif fg.gpu_error:
+ self.get_logger().warn(
+ f"видеокарта не поднялась ({fg.gpu_error}) — считаю на процессоре")
+ else:
+ self._say_cpu()
+
+ def _publish_diag(self, res, stamp) -> None:
+ msg = DiagnosticArray()
+ msg.header.stamp = stamp
+ st = DiagnosticStatus()
+ st.name = "flyguard"
+ st.hardware_id = "lidar"
+ total = res.total_ms
+ st.level = (DiagnosticStatus.OK if total < 90 else DiagnosticStatus.WARN)
+ st.message = f"{total:.1f} мс/кадр"
+ st.values = [KeyValue(key=k, value=f"{v:.2f}") for k, v in res.timings.items()]
+ st.values.append(KeyValue(key="candidates", value=str(len(res.candidates))))
+ st.values.append(KeyValue(key="tracks", value=str(len(self.fg.cx.tracks))))
+ st.values.append(KeyValue(key="dropped_frames", value=str(self._dropped)))
+ # считает сама нода: внешний подписчик тоже теряет сообщения и занижает оценку
+ st.values.append(KeyValue(key="frames_processed", value=str(self.fg.frames_seen)))
+ # Настоящий счётчик приёма, включая кадры, ушедшие на калибровку
+ # решётки: без них разница «принято минус обработано» выглядела
+ # потерей, хотя это цена восстановления геометрии лучей по данным.
+ st.values.append(KeyValue(key="frames_received", value=str(self._received)))
+ st.values.append(KeyValue(key="frames_calibration",
+ value=str(max(self._received - self.fg.frames_seen
+ - self._dropped, 0))))
+ st.values.append(KeyValue(key="cycle_ms", value=f"{self._cycle_ms:.1f}"))
+ st.values.append(KeyValue(key="device", value=self.fg.device))
+ if self._gpu_on and not self.fg.gpu_active:
+ # видеокарта отказала на ходу: кадр досчитан на процессоре, дальше — только он
+ self._gpu_on = False
+ self.get_logger().warn(
+ f"видеокарта отказала ({self.fg.gpu_error}) — дальше считаю на процессоре")
+ if res.ego:
+ st.values.append(KeyValue(key="speed_kmh", value=f"{res.ego.kmh:.1f}"))
+ st.values.append(KeyValue(key="rail_height_m", value=f"{res.plane.height:.3f}"))
+ st.values.append(KeyValue(key="curve_radius_m", value=f"{res.corridor.radius:.0f}"))
+ msg.status.append(st)
+ self.pub_diag.publish(msg)
+
+ def summary(self) -> str:
+ """Итог сеанса: по нему сразу видно, дошли ли кадры и сколько было тревог."""
+ seen = self.fg.frames_seen
+ calib = max(self._received - seen - self._dropped, 0)
+ return (f"итог: принято кадров {self._received}, обработано {seen}, "
+ f"на калибровку {calib}, пропущено {self._dropped}; "
+ f"кадров с тревогой {self._alarm_frames}")
+
+ def destroy_node(self) -> bool:
+ self._stop = True
+ self._wake.set()
+ return super().destroy_node()
+
+
+def main(argv=None) -> None:
+ rclpy.init(args=argv)
+ node = FlyGuardNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ # По Ctrl+C обработчик rclpy успевает закрыть контекст раньше, и журнал
+ # ROS тогда ругается «Failed to publish log message to rosout».
+ if rclpy.ok():
+ node.get_logger().info(node.summary())
+ else:
+ print(f"[flyguard] {node.summary()}", flush=True)
+ node.destroy_node()
+ rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2_ws/src/flyguard/flyguard/pipeline.py b/ros2_ws/src/flyguard/flyguard/pipeline.py
new file mode 100644
index 0000000..84fb2b6
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/pipeline.py
@@ -0,0 +1,614 @@
+"""Сборка конвейера: от облака точек до решения.
+
+Порядок стадий повторяет путь сигнала в мозге мухи:
+
+ ретина → стабилизация → ламина → медулла/лобулярная пластинка
+ → лобула → грибовидное тело → центральный комплекс → нисходящие нейроны
+
+Конвейер держит состояние между кадрами (поза, коридор, собственное движение,
+треки), поэтому один экземпляр обслуживает один поток лидарных данных.
+"""
+from __future__ import annotations
+
+import threading
+import time
+from dataclasses import dataclass, field
+
+import numpy as np
+
+from . import lamina
+from .cdr import PointCloud2
+from .central_complex import CentralComplex
+from .descending import Decision, DescendingNeurons
+from .geometry import (STRAIGHT, Corridor, RailPlane, TrackFrame, fit_corridor,
+ fit_rail_plane)
+from .fan_body import FanBody
+from .lobula import Candidate, cluster_by_depth, find_candidates, gauge_mask
+from .medulla import EgoMotion, EgoMotionEstimator, EmdBank, looming
+from .mbon_readout import MbonReadout
+from .mushroom_body import Habituation, HabituationConfig, MushroomBody, NoMemory
+from .retina import RangeImage, ScanLayout
+
+
+@dataclass
+class Params:
+ """Все настройки конвейера в одном месте."""
+
+ fov_deg: float = 30.0 # полусектор обработки по азимуту
+ # Полуширина габарита. 1.2, а не 1.6: так габарит задают организаторы в
+ # синтетике 24.09 — «у края» кончается на 1.13 м от оси, «вне габарита,
+ # но близко» начинается с 1.14. При 1.6 второй давал ложную тревогу, а
+ # в `doubleT_obstacle` в полосу заходили колонны между путями. Полигон
+ # парно +43 / −33 наблюдения, посторонних 793 → 742 (EXPERIMENTS п. 18.4).
+ half_width: float = 1.2
+ h_lo: float = 0.28 # нижняя граница габарита над рельсом, м
+ # Три правки габарита (Zhirik1337, 22.09), каждая мерилась отдельно на
+ # двух сценах и полигоне (EXPERIMENTS п. 16):
+ # h_lo_core — пол в колее: между рельсами (|u| ≤ core_width) нижняя
+ # граница опускается, чтобы не терять лежащего человека.
+ # ПРИНЯТО, но только дальше core_from: ближе в полосу
+ # 0.16…0.28 м попадают головки рельсов, рельс собирается
+ # в одну компоненту от кабины, и предмет выбрасывается
+ # вместе с ней. Лежачий P@50 0.27 → 0.50;
+ # k_sigma — габарит расширяется там, где ось пути известна хуже
+ # (в кривых вдали); 0 — не расширять. Отвергнуто;
+ # platform_filter — вырезать из габарита настил платформы. Отвергнуто:
+ # в тоннеле без платформы режет стену на обрывки.
+ h_lo_core: float = 0.16
+ core_width: float = 0.85 # полуширина колеи, м
+ core_from: float = 30.0 # ближе этой дальности пол в колее не опускается, м
+ k_sigma: float = 0.0
+ platform_filter: bool = False
+ h_hi: float = 2.3 # верхняя граница, м
+ # Верхняя секция габарита: от h_hi до h_top при |u| < half_width_top —
+ # то, что свисает со свода в путь вагона (см. lobula.gauge_mask). Вагон
+ # выше 3.5 м, а предмет «2×2 сверху» у организаторов висит низом на 3.0 м
+ # над рельсом — при верхе 2.3 м он был не виден вовсе. Верх 3.3, а не 3.7:
+ # у свода в настоящих тоннелях свои кабели и кронштейны, и ошибка наклона
+ # плоскости пути на 60–90 м опускает их на 0.1–0.3 м — при 3.7 они шли в
+ # тревоги (низ у них в основном выше 3.2 м, у предметов организаторов —
+ # 2.8 и 3.0). h_top <= h_hi — секции нет. EXPERIMENTS п. 18.4, 19.
+ h_top: float = 3.3
+ half_width_top: float = 1.0
+ top_d_max: float = 90.0 # дальше верхней секции нет, м
+ # Плоское у пола (см. central_complex._quality): вертикальный размер меньше
+ # flat_h, верх ниже flat_top над рельсом, не дальше flat_d — вес наблюдения
+ # умножается на flat_w. flat_h: 0 — выключено. EXPERIMENTS п. 19.
+ flat_h: float = 0.06
+ # Отдельный проход по верхней секции для свисающего со свода (см.
+ # lobula._hanging): в основном проходе оно склеено со сводом.
+ top_detect: bool = True
+ # Дальний трек (дальше far_confirm_from) поднимает тревогу, только прожив
+ # far_confirm кадров (см. descending.py). 0 — выключено. Замерено и
+ # отвергнуто: 15 кадров убирают короткие дальние фантомы, но полигон теряет
+ # 200 наблюдений, P@150 у человека стоя 0.46 → 0.37. EXPERIMENTS п. 19.
+ far_confirm: int = 0
+ far_confirm_from: float = 100.0
+ flat_top: float = 0.26
+ flat_d: float = 70.0
+ flat_w: float = 0.15
+ ctx_up: float = 4.0 # насколько контекст лезет выше габарита, м
+ # Разделение фигуры и фона по движению (см. lobula.split_by_figure):
+ # порог скорости сближения вдоль луча, в долях пройденного пути.
+ # Поднимает рабочую дальность с 62 до 80 м, но вчетверо увеличивает ложные
+ # тревоги: в тоннеле полно поверхностей, которые приближаются так же, как
+ # предмет. 0 — выключено; рабочее значение 0.6. Кривая — EXPERIMENTS п. 9.4.
+ split_adv: float = 0.0
+ # Разделение фигуры и фона по контрасту ламины (см. lobula.split_by_figure):
+ # порог «насколько ближе своего окружения», м. Гладкая стена даёт ноль по
+ # построению центр-окружения, предмет на стене — ступеньку. Именно это
+ # вытаскивает предмет из компоненты, растёкшейся вдоль стены круглого
+ # тоннеля, где покадровый тракт слеп целиком. 0 — выключено.
+ split_gap: float = 6.0
+ split_near: float = 55.0 # ближе этого резать только растянутое
+ # Разброс дальности внутри компоненты, выше которого её режут и в
+ # ближней зоне: предмет по дальности занимает десятки сантиметров,
+ # кромка платформы и стена — десятки метров. 0 — не резать вблизи
+ # вовсе. Замер — EXPERIMENTS п. 12.7.
+ near_long: float = 0.0
+ split_top: int = 1 # сколько фигур выносить из одной компоненты
+ # Накопление слабых улик в координатах пути (см. fan_body.py). Заменяет
+ # недоступный на большой дальности контраст к фону: на 140…185 м `gap`
+ # структурно равен нулю, и без накопления улика не растёт вовсе.
+ enable_accumulator: bool = True
+ acc_near: float = 55.0 # ближе этого покадровый тракт и так справляется
+ acc_gain: float = 1.5 # опора накопителя, засчитываемая за полный контраст
+ # Привыкание внутри проезда (см. mushroom_body.Habituation). Замысел: гасить
+ # формы, встретившиеся в НЕСКОЛЬКИХ разных точках пути, то есть штатную
+ # повторяющуюся обстановку, без единого прошлого проезда.
+ #
+ # ПО УМОЛЧАНИЮ ВЫКЛЮЧЕНО — измерено, что избирательности у механизма нет.
+ # При ёмкости, достаточной чтобы популяция не насыщалась (20 000 клеток),
+ # привыкание не меняет ничего: 7.5 ложных трека на км против тех же 7.5.
+ # Весь эффект маленькой популяции (5.9 на км) оказался насыщением, которое
+ # давит без разбора: новизна вставленного предмета падала до 0.24 при
+ # медианной новизне кандидата 0.43…0.71, то есть предмет подавлялся
+ # СИЛЬНЕЕ обстановки. Огрубление признаков и резкий отсчёт MBON не помогли.
+ # Разбор — EXPERIMENTS п. 10.
+ enable_habituation: bool = False
+ hab_rate: float = 0.25 # депрессия за одно новое место
+ hab_place_m: float = 5.0 # насколько разнести места, чтобы считать разными
+ hab_recover_m: float = 800.0 # путь, за который привыкание сходит наполовину
+ hab_n_kc: int = 20000 # ёмкость короткой памяти, клеток Кеньона
+ hab_read_q: float = 0.75 # квантиль отсчёта MBON
+ hab_quant: float = 0.5 # огрубление признаков, в долях разброса
+ # Обученное считывание MBON (см. mbon_readout.py). Заменяет ручную формулу
+ # веса улики в центральном комплексе там, где модель передана конвейеру;
+ # без модели всё работает по-прежнему.
+ enable_mbon: bool = True
+ # Резкость: вес наблюдения p**power. Рабочая точка 1.5 выбрана сравнением
+ # при равной строгости (EXPERIMENTS п. 17.4): ложных треков на незнакомой
+ # линии 23.5 → 14.7 на км, на знакомой 6.5 → 5.7, на второй половине
+ # new_data 23.9 → 17.4, а полигон теряет 142 наблюдения из 7 600: 39
+ # вблизи у края габарита (пограничный случай п. 17.1–17.2) и 82 за 90 м.
+ # 2.0 срезает ещё 127 наблюдений ради 0.4 трека на км.
+ mbon_power: float = 1.5
+ mbon_blend: float = 1.0 # 1 — только модель, 0 — только ручная формула
+ # Гашение знакомости с дальностью: за `nov_fade_from` её вклад падает
+ # до нуля к `nov_fade_to`. За 90 м новизна измеримо ВРЕДНА — у
+ # вставленного человека на 120…185 м она 0.150 против 0.199 у
+ # обстановки, AUC 0.293, то есть признак перевёрнут. Гашение подняло
+ # P@100 с 0.31 до 0.48 и P@150 с нуля до 0.32 ценой 3.3 → 5.6 ложных
+ # трека на км на ЗНАКОМОЙ линии и ноля на незнакомой. 0 — не гасить,
+ # это тихий режим для линии с обученной памятью. EXPERIMENTS п. 12.3.
+ nov_fade_from: float = 90.0
+ nov_fade_to: float = 160.0
+ # Поправка оценки модели на распространённость предметов: за этой
+ # дальностью из логита вычитается типичный для полосы. 0 — не
+ # поправлять. Замер — EXPERIMENTS п. 12.3.
+ mbon_prior_from: float = 0.0
+ # Пороги решения (см. descending.py). Вынесены сюда, потому что обученное
+ # считывание меняет цену улики: когда ложные тревоги падают, запас можно
+ # потратить на дальность, опустив порог. Замер — EXPERIMENTS п. 11.
+ # Считывание по треку (см. track_readout.py): геометрическое смешивание
+ # обученного отсчёта с уликой перед сравнением с порогом. 0 — решает
+ # только улика, как раньше. Замер — EXPERIMENTS п. 15.
+ # Правки решения (Zhirik1337, 22.09), обе выключены по замеру (п. 16):
+ # lying_exempt — не штрафовать за вытянутость низкий предмет в колее
+ # (лежащий человек вытянут вдоль пути, как конструкция).
+ # Правит ручную формулу, а при mbon_blend = 1 вес целиком
+ # от модели — не действует;
+ # far_channel — дальний канал предупреждения в обход порога. Выигрыш
+ # настоящий, но та же задача, что warn_far, втрое дороже:
+ # +3.0 и +7.1 ложных трека на км на двух сценах.
+ lying_exempt: bool = False
+ far_channel: bool = False
+ track_blend: float = 0.0
+ track_gate: float = 0.0 # жёсткий порог по тому же отсчёту
+ # Чем считать «насколько это предмет» по треку: `w_mean` — средний вес
+ # наблюдения за всю жизнь, `w_ema` — он же со скользящим забыванием,
+ # `p_mean` — средний покадровый отсчёт считывания, `model` — обученное
+ # считывание по треку (нужен файл модели). Замер — п. 15.
+ track_score: str = "w_mean"
+ warn_evidence: float = 0.5 # улика, с которой трек поднимает тревогу
+ clear_evidence: float = 0.3 # и до которой держится (гистерезис)
+ emergency_evidence: float = 0.75
+ # Наблюдений, без которых трек не считается. Три подтверждения стоили
+ # дальности на предметах среднего размера: на 60…100 м они видны через
+ # кадр, и третьего приходится ждать слишком долго. Два подтверждения
+ # подняли рабочую дальность человека сидя с 20 до 80 м, а чемодана —
+ # с 20 до 62 м, и не стоили ни одного лишнего ложного трека
+ # (3.3 на км в обоих случаях). Замер — EXPERIMENTS п. 11.3.
+ min_hits: int = 2
+ novelty_floor: float = 0.10
+ # Порог тревоги, линейно опускаемый с 0.5 до `warn_far` на отрезке от
+ # `warn_far_from` до 200 м. В п. 12.2 ровно эта правка была отвергнута:
+ # тогда улика далёкого трека колебалась около нуля, и опускать порог под
+ # нулевым сигналом было бессмысленно. После гашения знакомости и порога
+ # по лучам улика перестала быть нулём, и та же правка дала P@150
+ # 0.33 → 0.37 без потерь на 50 и 100 м ценой нуля ложных треков на
+ # знакомой линии и одного на незнакомой. Экстренного торможения она не
+ # касается: у него свой порог и условие d ≤ max(тормозной путь, 25 м).
+ # EXPERIMENTS п. 15.9.
+ warn_far: float = 0.30
+ warn_far_from: float = 90.0
+ leak_far: float = 0.12 # утечка улики на дальнем краю; равна leak — выключено
+ leak_far_from: float = 90.0 # с какой дальности она начинает падать
+ d_min: float = 4.0
+ d_max: float = 220.0
+ # Сколько лучей образуют кандидата. Вблизи обрывок из трёх лучей — это
+ # кусок чего-то большего, и он дробит настоящий предмет: разрешить
+ # тройки везде стоит P@50 у человека 0.70 → 0.64. Вдали три луча —
+ # всё, что есть (медиана на 150 м — шесть). Порог по дальности берёт
+ # дальний выигрыш целиком (P@150 0.32 → 0.37, человек сидя 80 → 100 м
+ # рабочей дальности) и не стоит ни одного лишнего ложного трека ни на
+ # знакомой линии, ни на незнакомой. EXPERIMENTS п. 12.5.
+ min_rays: int = 4
+ min_rays_far: int = 3 # 0 — не различать
+ min_rays_far_from: float = 90.0
+ calib_frames: int = 12
+ # Скачок времени записи больше этого (или назад) — перемотка, повтор или
+ # выпадение данных: треки, одометрия и ось пути начинаются заново. 0 — не
+ # сбрасывать никогда.
+ reset_gap_s: float = 2.0
+ novelty_gate: float = 0.0 # ниже этой новизны кандидат отбрасывается сразу
+ # Где считать плотные стадии — сетчатку, ламину, кластеризацию (gpu.py):
+ # 'auto' — видеокарта, если PyTorch её видит, иначе процессор; 'cuda';
+ # 'cpu'. Результат одинаков: на видеокарте те же операции в тех же типах.
+ # Любой сбой видеокарты переводит конвейер на процессор до конца работы,
+ # кадр досчитывается там же. Узел поднимает видеокарту в фоне
+ # (`gpu_background`), считая первые кадры на процессоре. EXPERIMENTS п. 21.
+ device: str = "auto"
+ enable_motion: bool = True
+ enable_memory: bool = True
+ enable_looming: bool = False # T4/T5 + LPLC2: нужны для оценки надвигания и для
+ # схемы мозга; на решение пока не влияют
+
+ # Переключатели для абляции: каждый отключает один механизм, оставляя
+ # остальные. Нужны, чтобы вклад механизма измерялся, а не декларировался.
+ use_corridor: bool = True # ось пути; False — прямой коридор вперёд
+ use_shape: bool = True # целостность, компактность, опора снизу
+ use_tracking: bool = True # накопление улик по кадрам
+
+
+@dataclass
+class FrameResult:
+ """Всё, что конвейер узнал за один кадр."""
+
+ stamp: float
+ decision: Decision
+ candidates: list[Candidate]
+ ego: EgoMotion | None
+ plane: RailPlane
+ corridor: Corridor
+ tf: TrackFrame | None = None
+ lam: lamina.LaminaOutput | None = None
+ emd: np.ndarray | None = None # (4, h, w) отклики T4/T5
+ loom: np.ndarray | None = None # (h, w) отклик LPLC2
+ hab: float = 0.0 # привыкание популяции KC, 0…1
+ timings: dict = field(default_factory=dict)
+
+ @property
+ def total_ms(self) -> float:
+ return sum(self.timings.values())
+
+
+class FlyGuard:
+ """Полный конвейер обнаружения посторонних объектов."""
+
+ def __init__(self, params: Params | None = None,
+ memory: MushroomBody | None = None,
+ layout: ScanLayout | None = None,
+ readout: "MbonReadout | None" = None,
+ track_readout=None,
+ gpu_background: bool = False):
+ self.p = params or Params()
+ self.device = "cpu"
+ self.gpu = None # плотные стадии на видеокарте (gpu.GpuStages)
+ self.gpu_error: str | None = None
+ self._gpu_ready = None # (стадии, устройство) — готовы, ждут начала кадра
+ # gpu_background: видеокарта поднимается в своём потоке, а кадры тем
+ # временем считает процессор. Первый запуск ядер CUDA бывает долгим —
+ # в Docker под WSL 15 с на каждый новый контейнер, — и узел всё это время
+ # не был подписан и терял кадры. Зависни драйвер совсем — узел так и
+ # останется на процессоре, а не встанет.
+ self.gpu_pending = str(self.p.device).strip().lower() != "cpu"
+ if self.gpu_pending:
+ if gpu_background:
+ threading.Thread(target=self._bring_up_gpu, name="flyguard-gpu",
+ daemon=True).start()
+ else:
+ self._bring_up_gpu()
+ self._take_gpu()
+ self.memory = memory if (memory is not None and self.p.enable_memory) else NoMemory()
+ self.readout = readout if (readout is not None and self.p.enable_mbon) else None
+ self.layout_full = layout
+ self.layout: ScanLayout | None = None
+ self.cols: slice | None = None
+ self._track_readout = track_readout
+ self._init_temporal()
+ self._calib: list[PointCloud2] = []
+ self.frames_seen = 0
+ self._last_stamp: float | None = None
+ self.time_jumps = 0 # сколько раз время записи скакало
+
+ def _init_temporal(self) -> None:
+ """Всё, что копится от кадра к кадру: треки, одометрия, ось, улики.
+
+ Решётка лучей сюда не входит — она от сенсора, а не от места на пути.
+ """
+ track_readout = self._track_readout
+ self.plane: RailPlane | None = None
+ self._prev_r: np.ndarray | None = None
+ self.corridor: Corridor | None = None
+ self.ego_est = EgoMotionEstimator()
+ self.fan: FanBody | None = None
+ self.s_path = 0.0 # пройденный путь, м — координата привыкания
+ self.hab = (Habituation(HabituationConfig(
+ n_kc=self.p.hab_n_kc, sparsity=80.0 / max(self.p.hab_n_kc, 1),
+ rate=self.p.hab_rate, place_m=self.p.hab_place_m,
+ read_q=self.p.hab_read_q, quant=self.p.hab_quant,
+ recover_m=self.p.hab_recover_m))
+ if self.p.enable_habituation else None)
+ self.emd_bank = EmdBank()
+ self.cx = CentralComplex(use_shape=self.p.use_shape,
+ mbon_power=self.p.mbon_power,
+ mbon_blend=self.p.mbon_blend,
+ leak_far=self.p.leak_far,
+ leak_far_from=self.p.leak_far_from,
+ nov_fade_from=self.p.nov_fade_from,
+ nov_fade_to=self.p.nov_fade_to,
+ lying_exempt=self.p.lying_exempt,
+ top_from=(self.p.h_hi if self.p.h_top > self.p.h_hi
+ else float("inf")),
+ flat=(self.p.flat_h, self.p.flat_top, self.p.flat_d,
+ self.p.flat_w))
+ # без накопления улик трек подтверждается первым же наблюдением
+ self.dn = (DescendingNeurons(
+ warn_evidence=self.p.warn_evidence,
+ clear_evidence=self.p.clear_evidence,
+ emergency_evidence=self.p.emergency_evidence,
+ min_hits=self.p.min_hits,
+ novelty_floor=self.p.novelty_floor,
+ warn_far=self.p.warn_far,
+ warn_far_from=self.p.warn_far_from,
+ track_blend=self.p.track_blend,
+ track_gate=self.p.track_gate,
+ track_score=self.p.track_score,
+ track_readout=track_readout,
+ far_channel=self.p.far_channel,
+ far_confirm=self.p.far_confirm,
+ far_confirm_from=self.p.far_confirm_from) if self.p.use_tracking
+ else DescendingNeurons(warn_evidence=0.0, clear_evidence=0.0,
+ emergency_evidence=0.0, min_hits=1))
+
+ # ------------------------------------------------------------------ видеокарта
+
+ def _bring_up_gpu(self) -> None:
+ """Проверить CUDA, прогреть стадии и отдать их конвейеру (`_take_gpu`)."""
+ try:
+ from .device import get_device
+ dev = get_device(self.p.device)
+ if dev.startswith("cuda"):
+ from .gpu import GpuStages
+ stages = GpuStages(dev)
+ stages.warmup()
+ self._gpu_ready = (stages, dev)
+ except Exception as e: # нет драйвера, не та видеокарта, нет памяти
+ self._gpu_off(e)
+ finally:
+ self.gpu_pending = False
+
+ def _take_gpu(self) -> None:
+ """Перейти на видеокарту, если она готова. Только между кадрами.
+
+ Посреди кадра нельзя: ламина и кластеризация берут образ, который
+ сетчатка оставила на видеокарте.
+ """
+ if self._gpu_ready is not None:
+ self.gpu, self.device = self._gpu_ready
+ self._gpu_ready = None
+
+ @property
+ def gpu_active(self) -> bool:
+ """Плотные стадии на видеокарте — уже или с ближайшего кадра."""
+ return self.gpu is not None or self._gpu_ready is not None
+
+ def _gpu_off(self, exc: Exception) -> None:
+ """Сбой видеокарты: до конца работы считать на процессоре."""
+ from .device import notify_cuda_error
+ notify_cuda_error(exc)
+ self.gpu = None
+ self.device = "cpu"
+ self.gpu_error = f"{type(exc).__name__}: {exc}"
+
+ def _cluster(self, mask: np.ndarray, r: np.ndarray, **kw):
+ """Кластеризация с учётом глубины: видеокарта, при сбое — процессор."""
+ if self.gpu is not None:
+ try:
+ return self.gpu.cluster_by_depth(mask, r, **kw)
+ except Exception as e:
+ self._gpu_off(e)
+ return cluster_by_depth(mask, r, **kw)
+
+ # ------------------------------------------------------------------ калибровка
+
+ def _ensure_layout(self, pc: PointCloud2) -> bool:
+ """Накопить кадры и откалибровать решётку. False — ещё не готов."""
+ if self.layout is not None:
+ return True
+ if self.layout_full is None:
+ self._calib.append(pc)
+ if len(self._calib) < self.p.calib_frames:
+ return False
+ self.layout_full = ScanLayout.calibrate(self._calib)
+ self._calib.clear()
+ self.cols = self.layout_full.column_slice(self.p.fov_deg)
+ self.layout = self.layout_full.sub(self.cols)
+ return True
+
+ # ------------------------------------------------------------------ накопление
+
+ def _accumulate(self, tf, inside, lat, ds: float,
+ cands: list[Candidate]) -> None:
+ """Досыпать кадр в веерное тело и проставить опору каждому кандидату."""
+ if not self.p.enable_accumulator:
+ return
+ if self.fan is None:
+ self.fan = FanBody(d_max=self.p.d_max, half_width=self.p.half_width,
+ h_lo=self.p.h_lo, h_hi=self.p.h_hi,
+ d_near=self.p.acc_near)
+ self.fan.update(tf.d[inside], lat[inside], tf.h[inside], ds)
+ if not cands:
+ return
+ d = np.array([c.d for c in cands], np.float32)
+ u = np.array([c.u for c in cands], np.float32)
+ g = max(self.p.acc_gain, 1e-3)
+ for c, s in zip(cands, self.fan.support_at(d, u)):
+ c.extra["acc_support"] = float(s) / g
+
+ # ------------------------------------------------------------------ фигура и фон
+
+ MIN_DS = 0.25 # м: меньше — шум дальности забивает разницу
+
+ def _advance(self, tf, ds: float):
+ """Скорость сближения вдоль каждого луча, в долях пройденного пути.
+
+ Вдоль фиксированного луча стена, параллельная движению, не приближается
+ вовсе: поезд едет, точка пересечения скользит по стене, дальность
+ остаётся прежней. Предмет, обращённый к поезду, приближается ровно на
+ пройденный путь. Отсюда 0 у фона и 1 у фигуры.
+
+ Сравниваются два соседних кадра в одной решётке: столбец отвечает
+ фиксированному азимуту, поэтому достаточно вычитания без перепроекции.
+ Рысканье в кривой за кадр меньше шага решётки (0.06° при радиусе
+ 1300 м против шага 0.1°), поэтому им пренебрегаем.
+ """
+ cur = np.where(tf.valid, tf.r, np.nan).astype(np.float32)
+ prev, self._prev_r = self._prev_r, cur
+ if prev is None or prev.shape != cur.shape or ds < self.MIN_DS:
+ return None
+ with np.errstate(invalid="ignore"):
+ a = (prev - cur) / ds
+ return np.nan_to_num(a, nan=0.0, posinf=0.0, neginf=0.0)
+
+ # ------------------------------------------------------------------ основной такт
+
+ def process(self, pc: PointCloud2, *, keep_debug: bool = False) -> FrameResult | None:
+ """Обработать один кадр. None, пока идёт калибровка решётки."""
+ t = _Timer()
+ if not self._ensure_layout(pc):
+ return None
+ # Скачок времени записи — перемотка, `ros2 bag play --loop`, разрыв в
+ # записи. Треки, одометрия и ось пути относятся к другому месту, и
+ # с ними предмет с прошлого круга всплыл бы там, где его нет.
+ if (self._last_stamp is not None and self.p.reset_gap_s > 0
+ and not (-0.05 < pc.stamp - self._last_stamp < self.p.reset_gap_s)):
+ self._init_temporal()
+ self.time_jumps += 1
+ self._last_stamp = pc.stamp
+ self.frames_seen += 1
+ self._take_gpu()
+
+ with t("retina"):
+ img: RangeImage | None = None
+ if self.gpu is not None:
+ try:
+ img = self.gpu.project(self.layout_full, pc, self.cols)
+ except Exception as e:
+ self._gpu_off(e)
+ if img is None:
+ img = self.layout_full.project(pc, self.cols)
+
+ with t("stabilize"):
+ self.plane = fit_rail_plane(img, self.layout, prev=self.plane)
+ tf = TrackFrame(img, self.layout, self.plane)
+
+ with t("corridor"):
+ self.corridor = (fit_corridor(tf, prev=self.corridor) if self.p.use_corridor
+ else STRAIGHT)
+
+ with t("lamina"):
+ lam = None
+ if self.gpu is not None:
+ try:
+ lam = self.gpu.lamina() # по образу кадра, уже лежащему на видеокарте
+ except Exception as e:
+ self._gpu_off(e)
+ if lam is None:
+ lam = lamina.process(tf.r, tf.valid, device="cpu")
+
+ with t("ego"):
+ ego = (self.ego_est.update(tf, pc.stamp) if self.p.enable_motion
+ else EgoMotion(0.0, 0.0, 0.0, 0.0, 0.1))
+
+ with t("figure"):
+ advance = self._advance(tf, ego.ds)
+
+ emd = loom = None
+ if self.p.enable_looming:
+ with t("medulla"):
+ emd = self.emd_bank.update(lam.on)
+ loom = looming(emd)
+
+ with t("lobula"):
+ masks = gauge_mask(tf, self.corridor, half_width=self.p.half_width,
+ h_lo=self.p.h_lo, h_hi=self.p.h_hi,
+ d_min=self.p.d_min, d_max=self.p.d_max,
+ k_sigma=self.p.k_sigma,
+ h_lo_core=self.p.h_lo_core,
+ core_width=self.p.core_width,
+ core_from=self.p.core_from,
+ platform_filter=self.p.platform_filter,
+ h_top=self.p.h_top,
+ half_width_top=self.p.half_width_top,
+ top_d_max=self.p.top_d_max)
+ cands = find_candidates(tf, lam, self.corridor, masks=masks,
+ half_width=self.p.half_width, h_lo=self.p.h_lo,
+ h_hi=self.p.h_hi, ctx_up=self.p.ctx_up,
+ split_adv=self.p.split_adv,
+ split_gap=self.p.split_gap,
+ split_near=self.p.split_near,
+ near_long=self.p.near_long,
+ split_top=self.p.split_top, advance=advance,
+ d_min=self.p.d_min,
+ d_max=self.p.d_max, min_rays=self.p.min_rays,
+ min_rays_far=self.p.min_rays_far,
+ min_rays_far_from=self.p.min_rays_far_from,
+ k_sigma=self.p.k_sigma,
+ h_lo_core=self.p.h_lo_core,
+ core_width=self.p.core_width,
+ core_from=self.p.core_from,
+ platform_filter=self.p.platform_filter,
+ h_top=self.p.h_top,
+ half_width_top=self.p.half_width_top,
+ top_d_max=self.p.top_d_max,
+ top_detect=self.p.top_detect,
+ cluster=self._cluster)
+
+ with t("mushroom"):
+ cands = self.memory.annotate(cands)
+ self.s_path += ego.ds
+ if self.hab is not None:
+ self.hab.update(cands, self.s_path, ego.ds)
+ if self.p.novelty_gate > 0:
+ cands = [c for c in cands if c.novelty >= self.p.novelty_gate]
+
+ with t("fan"):
+ # Накопитель идёт ПОСЛЕ памяти и мимо неё: память обучена на
+ # покадровых кандидатах, а у накопленного скопления нет ни
+ # контраста к фону, ни интенсивности — её признаки для него
+ # не определены. Судить о нём надо по тому, что у него есть:
+ # по числу попаданий в одну точку пути и по размеру.
+ self._accumulate(tf, masks[0], masks[1], ego.ds, cands)
+
+ with t("readout"):
+ # Строго после накопителя: опора веерного тела входит в признаки
+ # считывания, и на большой дальности это его главный вход.
+ if self.readout is not None:
+ self.readout.annotate(cands, self.p.mbon_prior_from)
+
+ with t("central"):
+ self.cx.update(cands, ego.ds, ego.dt)
+
+ with t("descending"):
+ decision = self.dn.decide(self.cx, ego.speed)
+
+ return FrameResult(stamp=pc.stamp, decision=decision, candidates=cands,
+ ego=ego, plane=self.plane, corridor=self.corridor,
+ tf=tf if keep_debug else None,
+ lam=lam if keep_debug else None,
+ emd=emd, loom=loom,
+ hab=self.hab.level if self.hab is not None else 0.0,
+ timings=t.result)
+
+
+class _Timer:
+ """Замер времени по стадиям — нужен для отчёта о задержке."""
+
+ def __init__(self):
+ self.result: dict[str, float] = {}
+ self._name = None
+ self._t0 = 0.0
+
+ def __call__(self, name: str):
+ self._name = name
+ return self
+
+ def __enter__(self):
+ self._t0 = time.perf_counter()
+ return self
+
+ def __exit__(self, *exc):
+ self.result[self._name] = (time.perf_counter() - self._t0) * 1e3
+ return False
diff --git a/ros2_ws/src/flyguard/flyguard/player_keys.py b/ros2_ws/src/flyguard/flyguard/player_keys.py
new file mode 100644
index 0000000..1fe0d3f
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/player_keys.py
@@ -0,0 +1,183 @@
+"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
+
+`ros2 bag play`, запущенный из launch (`bag:=...`), клавиатуры не слышит — у
+него нет терминала. Пульт управляет им через сервисы rosbag2 из второго
+терминала того же контейнера:
+
+ docker exec -it flyguard_demo flyguard-keys /data/<запись>
+
+ пробел пауза / продолжить
+ ← → на 3 секунды назад / вперёд
+ . один кадр вперёд (на паузе)
+ ↑ ↓ быстрее / медленнее
+ 0 в начало записи
+ q выйти (запись играет дальше)
+
+Текущее время записи берётся из /clock — launch запускает проигрыватель с
+`--clock`. Путь к записи нужен только для клавиши «0»: из metadata.yaml
+читается время её начала. Узел детектора после перемотки сам сбрасывает треки
+и одометрию (скачок времени, `pipeline.FlyGuard.process`).
+"""
+from __future__ import annotations
+
+import re
+import select
+import sys
+import termios
+import tty
+from pathlib import Path
+
+import rclpy
+from builtin_interfaces.msg import Time
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from rosbag2_interfaces.srv import GetRate, IsPaused, PlayNext, Seek, SetRate, TogglePaused
+from rosgraph_msgs.msg import Clock
+
+STEP_S = 3.0
+RATES = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0]
+PLAYER = "/rosbag2_player/"
+KEYS = """ пробел пауза / продолжить
+ ← → на 3 секунды назад / вперёд
+ . один кадр вперёд (на паузе)
+ ↑ ↓ быстрее / медленнее
+ 0 в начало записи
+ q выйти (запись играет дальше)
+"""
+
+
+def bag_span(path: str | None) -> tuple[float | None, float | None]:
+ """Начало и длительность записи из metadata.yaml, секунды."""
+ if not path:
+ return None, None
+ meta = Path(path) / "metadata.yaml"
+ if not meta.exists():
+ return None, None
+ txt = meta.read_text(encoding="utf-8")
+ s = re.search(r"starting_time:\s*\n\s*nanoseconds_since_epoch:\s*(\d+)", txt)
+ d = re.search(r"duration:\s*\n\s*nanoseconds:\s*(\d+)", txt)
+ return (int(s.group(1)) * 1e-9 if s else None, int(d.group(1)) * 1e-9 if d else None)
+
+
+class PlayerKeys(Node):
+ def __init__(self, start: float | None, duration: float | None):
+ super().__init__("flyguard_player_keys")
+ self.now: float | None = None
+ self.start = start
+ self.duration = duration
+ self.state, self.rate = "идёт", 1.0
+ # /clock проигрыватель шлёт без подтверждений (best effort): с надёжной
+ # подпиской QoS не сходится, и время не приходит вовсе
+ self.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data)
+ self.cli = {name: self.create_client(srv, PLAYER + name) for name, srv in (
+ ("toggle_paused", TogglePaused), ("is_paused", IsPaused), ("seek", Seek),
+ ("set_rate", SetRate), ("get_rate", GetRate), ("play_next", PlayNext))}
+
+ def _on_clock(self, msg: Clock) -> None:
+ self.now = msg.clock.sec + msg.clock.nanosec * 1e-9
+ if self.start is None:
+ self.start = self.now
+
+ def call(self, name: str, req=None):
+ cli = self.cli[name]
+ if not cli.wait_for_service(timeout_sec=2.0):
+ self.say("проигрыватель не отвечает — запись закончилась или запущена без launch?")
+ return None
+ fut = cli.call_async(req if req is not None else cli.srv_type.Request())
+ rclpy.spin_until_future_complete(self, fut, timeout_sec=3.0)
+ return fut.result()
+
+ def seek(self, t: float) -> None:
+ # за конец записи проигрыватель просто закончит её — не пускаем
+ if self.start is not None:
+ t = max(t, self.start)
+ if self.duration:
+ t = min(t, self.start + self.duration - 0.5)
+ req = Seek.Request()
+ sec = int(t)
+ req.time = Time(sec=sec, nanosec=int((t - sec) * 1e9))
+ self.call("seek", req)
+
+ def set_rate(self, up: bool) -> None:
+ got = self.call("get_rate")
+ cur = got.rate if got else 1.0
+ if up:
+ new = next((r for r in RATES if r > cur + 1e-6), RATES[-1])
+ else:
+ new = next((r for r in reversed(RATES) if r < cur - 1e-6), RATES[0])
+ req = SetRate.Request()
+ req.rate = float(new)
+ self.call("set_rate", req)
+
+ def status(self, ask: bool = True) -> str:
+ """Строка состояния; ask=False — без запросов к проигрывателю."""
+ if ask:
+ paused = self.call("is_paused")
+ rate = self.call("get_rate")
+ self.state = "ПАУЗА" if paused and paused.paused else "идёт"
+ self.rate = rate.rate if rate else 1.0
+ pos = (f"{self.now - self.start:6.1f} с" if self.now is not None and self.start
+ else " ? с")
+ return f"{pos} {self.state:6s} скорость ×{self.rate:g}"
+
+ @staticmethod
+ def say(text: str) -> None:
+ sys.stdout.write("\r\033[K" + text)
+ sys.stdout.flush()
+
+
+def read_key(fd: int) -> str:
+ ch = sys.stdin.read(1)
+ if ch != "\x1b":
+ return ch
+ if select.select([fd], [], [], 0.05)[0]:
+ seq = sys.stdin.read(2)
+ return {"[A": "up", "[B": "down", "[C": "right", "[D": "left"}.get(seq, "")
+ return "esc"
+
+
+def main(argv=None) -> None:
+ args = [a for a in (argv or sys.argv[1:]) if not a.startswith("--")]
+ rclpy.init()
+ node = PlayerKeys(*bag_span(args[0] if args else None))
+ print(KEYS)
+ fd = sys.stdin.fileno()
+ old = termios.tcgetattr(fd)
+ try:
+ tty.setcbreak(fd)
+ node.say(node.status())
+ while rclpy.ok():
+ if not select.select([fd], [], [], 0.2)[0]:
+ rclpy.spin_once(node, timeout_sec=0.0)
+ node.say(node.status(ask=False)) # бегущее время записи
+ continue
+ key = read_key(fd)
+ rclpy.spin_once(node, timeout_sec=0.05)
+ if key in ("q", "й", "esc"):
+ break
+ if key == " ":
+ node.call("toggle_paused")
+ elif key in ("left", "right") and node.now is not None:
+ node.seek(node.now + (STEP_S if key == "right" else -STEP_S))
+ elif key in ("left", "right"):
+ node.say("ещё нет времени записи (/clock) — подождите секунду")
+ continue
+ elif key in (".", "ю"):
+ node.call("play_next")
+ elif key in ("up", "down"):
+ node.set_rate(key == "up")
+ elif key == "0" and node.start is not None:
+ node.seek(node.start)
+ rclpy.spin_once(node, timeout_sec=0.1)
+ node.say(node.status())
+ except KeyboardInterrupt:
+ pass
+ finally:
+ termios.tcsetattr(fd, termios.TCSADRAIN, old)
+ print()
+ node.destroy_node()
+ rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2_ws/src/flyguard/flyguard/retina.py b/ros2_ws/src/flyguard/flyguard/retina.py
new file mode 100644
index 0000000..d97cd39
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/retina.py
@@ -0,0 +1,590 @@
+"""RETINA — омматидиальная решётка.
+
+Фасеточный глаз дрозофилы — регулярная решётка омматидиев, каждый смотрит в свою
+фиксированную сторону. Вращающийся лидар устроен так же: пара (кольцо, столбец)
+задаёт направление луча. Поэтому облако точек сразу переводится в *ретинотопический*
+дальностный образ `(кольцо, азимут)`, и вся дальнейшая обработка идёт в этой
+решётке — как в зрительной системе мухи, а не в неупорядоченном облаке.
+
+Три особенности конкретного сенсора, измеренные по данным (см. `docs/ALGORITHM.md`):
+
+1. **Раскладка различается между бэгами**: 3600 азимутов на 360° против 1200 на
+ 100°. Решётка калибруется по самим данным, ничего не захардкожено.
+2. **Двойное эхо**: соседние столбцы делят один азимут. Когда эхо одно, оба слота
+ содержат одно значение; когда два — ближнее несёт объект, дальнее фон за ним.
+ Реально различаются ~3 % лучей, и это именно тонкие предметы и кромки.
+3. **Скос решётки**: у каждого лазерного канала свой постоянный азимутальный сдвиг,
+ разброс достигает **15.5°** (≈155 столбцов). В сыром виде «столбец» не является
+ направлением: соседние кольца одного столбца смотрят в стороны, разнесённые на
+ градусы. Поэтому образ **выпрямляется** целочисленным сдвигом строк; остаточная
+ ошибка < половины шага азимута и учитывается в таблице направлений.
+
+Быстрый путь опирается на порядок точек (столбец · эхо · кольцо). В синтетическом
+бэге организаторов он соблюдается не везде: у облака нет поля `ring`, а там, где
+вставлен предмет, заслонённые им точки удалены, а точки предмета вписаны в
+середину массива — всё, что дальше, сдвинуто (так в половине кадров). Поэтому
+порядок проверяется в каждом кадре: у кадра с целым порядком элевация каждой
+точки совпадает с элевацией её кольца до 0.0001°. Кадр, где это не так,
+раскладывается в ту же решётку **по углам каждой точки** — медленнее, зато без
+допущений о порядке. Калибровка без поля `ring` находит кольца по гистограмме
+элевации и берёт только кадры с целым порядком, а если таких нет — строит
+решётку целиком по углам.
+"""
+from __future__ import annotations
+
+import warnings
+from contextlib import contextmanager
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+
+from .cdr import PointCloud2
+
+DEG = np.pi / 180.0
+
+# Допуск проверки порядка точек: в целом кадре элевация точки совпадает с
+# элевацией её кольца до 0.0001° (замерено на всех бэгах и на синтетике), а
+# ближайшие кольца Pandar128 разнесены на 0.086°. Вставленные точки предмета
+# уходят от колец на 0.03° и больше, сдвинутые — на целое кольцо.
+ORDER_TOL_DEG = 0.01
+
+
+@dataclass
+class RangeImage:
+ """Выпрямленный дальностный образ в координатах `(кольцо, азимут)`."""
+
+ stamp: float
+ r_near: np.ndarray # (H, W) float32 — ближнее эхо, 0 = нет эха
+ r_far: np.ndarray # (H, W) float32 — дальнее эхо, 0 = нет эха
+ inten: np.ndarray # (H, W) float32 — интенсивность ближнего эха
+ valid: np.ndarray # (H, W) bool
+
+ @property
+ def shape(self) -> tuple[int, int]:
+ return self.r_near.shape
+
+ def xyz(self, layout: "ScanLayout") -> np.ndarray:
+ """Декартовы координаты ближнего эха: (H, W, 3)."""
+ return layout.dirs * self.r_near[..., None]
+
+ def crop(self, cols: slice) -> "RangeImage":
+ return RangeImage(self.stamp, self.r_near[:, cols], self.r_far[:, cols],
+ self.inten[:, cols], self.valid[:, cols])
+
+
+class ScanLayout:
+ """Калиброванная и выпрямленная решётка лучей.
+
+ Азимут отсчитывается от направления движения (вперёд = −Y в системе сенсора),
+ положительный вправо; элевация — вверх от горизонтали сенсора.
+ """
+
+ def __init__(self, el_deg: np.ndarray, az_step_deg: float, az0_deg: float,
+ col_shift: np.ndarray, az_resid_deg: np.ndarray,
+ n_az: int, n_echo: int, wrap: bool = False, indexed: bool = True):
+ self.el_deg = np.asarray(el_deg, np.float64) # (H,)
+ self.az_step_deg = float(az_step_deg) # шаг на азимутальный индекс
+ self.az0_deg = float(az0_deg)
+ self.col_shift = np.asarray(col_shift, np.int64) # (H,) выпрямление
+ self.az_resid_deg = np.asarray(az_resid_deg, np.float64) # (H,) остаток < шага/2
+ self.n_az = int(n_az)
+ self.n_echo = int(n_echo)
+ self.wrap = bool(wrap) # круговой скан: края смыкаются
+ # False — порядок точек неизвестен (решётка построена по углам), и
+ # каждый кадр раскладывается по углам точек
+ self.indexed = bool(indexed)
+ self.n_rings = self.el_deg.size
+ self.n_points = self.n_az * self.n_echo * self.n_rings
+ self.n_geometric = 0 # сколько кадров пришлось раскладывать по углам
+ self._init_lookup()
+
+ self.az_grid_deg = self.az0_deg + self.az_step_deg * np.arange(self.n_az)
+
+ # карта выборки для выпрямления: out[h, j] = raw[h, j + shift[h]]
+ j = np.arange(self.n_az)[None, :]
+ src = j + self.col_shift[:, None]
+ if self.wrap:
+ # у кругового скана «выехавшие» столбцы приходят с другого края
+ self.gather = np.mod(src, self.n_az).astype(np.intp)
+ self.gather_ok = np.ones_like(self.gather, bool)
+ else:
+ self.gather_ok = (src >= 0) & (src < self.n_az)
+ self.gather = np.clip(src, 0, self.n_az - 1).astype(np.intp)
+
+ self.az_full_deg = self.az_grid_deg[None, :] + self.az_resid_deg[:, None]
+ self.dirs = self._unit_dirs().astype(np.float32) # (H, W, 3)
+ # угловой шаг по кольцам: у решётки из одного кольца градиента нет
+ self.el_step_deg = (np.abs(np.gradient(self.el_deg)) if self.n_rings > 1
+ else np.full(self.n_rings, 0.125))
+
+ # ------------------------------------------------------------------ геометрия
+
+ def _unit_dirs(self) -> np.ndarray:
+ az = self.az_full_deg * DEG
+ el = (self.el_deg[:, None] * DEG) * np.ones_like(az)
+ c = np.cos(el)
+ return np.stack([c * np.sin(az), -c * np.cos(az), np.sin(el)], axis=-1)
+
+ def _init_lookup(self) -> None:
+ """Таблицы для проверки порядка точек и поиска кольца по элевации."""
+ self._sin_el = np.sin(self.el_deg * DEG).astype(np.float32)
+ self._el_order = np.argsort(self.el_deg)
+ self._el_asc = self.el_deg[self._el_order]
+
+ def column_slice(self, half_fov_deg: float) -> slice:
+ """Непрерывный диапазон столбцов внутри ±half_fov по азимуту."""
+ inside = np.flatnonzero(np.abs(self.az_grid_deg) <= half_fov_deg)
+ if inside.size == 0:
+ return slice(0, self.n_az)
+ return slice(int(inside[0]), int(inside[-1]) + 1)
+
+ def sub(self, cols: slice) -> "ScanLayout":
+ """Урезанная по азимуту копия решётки (для обработки только переднего сектора)."""
+ start = cols.start or 0
+ stop = cols.stop if cols.stop is not None else self.n_az
+ out = ScanLayout.__new__(ScanLayout)
+ out.el_deg = self.el_deg
+ out.az_step_deg = self.az_step_deg
+ out.az0_deg = float(self.az_grid_deg[start])
+ out.col_shift = self.col_shift
+ out.az_resid_deg = self.az_resid_deg
+ out.n_az = stop - start
+ out.n_echo = self.n_echo
+ out.wrap = False # вырезанный сектор больше не смыкается
+ out.indexed = self.indexed
+ out.n_rings = self.n_rings
+ out.n_points = self.n_points
+ out.n_geometric = 0
+ out._init_lookup()
+ out.az_grid_deg = self.az_grid_deg[start:stop]
+ out.gather_ok = self.gather_ok[:, start:stop]
+ out.gather = self.gather[:, start:stop]
+ out.az_full_deg = self.az_full_deg[:, start:stop]
+ out.dirs = np.ascontiguousarray(self.dirs[:, start:stop])
+ out.el_step_deg = self.el_step_deg
+ return out
+
+ # ------------------------------------------------------------------ проекция
+
+ def project(self, pc: PointCloud2, cols: slice | None = None) -> RangeImage:
+ """Облако точек → выпрямленный дальностный образ.
+
+ `cols` задаёт нужный диапазон **выходных** столбцов. Тяжёлая
+ арифметика (корень по 900 тыс. точек) выполняется только над теми
+ сырыми столбцами, которые в этот диапазон попадут с учётом скоса
+ каналов, — на круговом скане это экономит почти всё время стадии.
+
+ Кадр с нарушенным порядком точек (см. докстроку модуля) раскладывается
+ по углам точек: результат тот же, что дал бы целый кадр.
+ """
+ w = self.n_az
+ start = 0 if cols is None else (cols.start or 0)
+ stop = w if cols is None else (cols.stop if cols.stop is not None else w)
+ if self.indexed and pc.n_points == self.n_points:
+ img = self._project_indexed(pc, start, stop)
+ if img is not None:
+ return img
+ self.n_geometric += 1
+ return self._project_geometric(pc, start, stop)
+
+ def _project_indexed(self, pc: PointCloud2, start: int, stop: int) -> RangeImage | None:
+ """Быстрый путь по порядку точек. None — порядок в кадре нарушен."""
+ n, w, e = self.n_rings, self.n_az, self.n_echo
+ lo = start + int(self.col_shift.min())
+ hi = stop + int(self.col_shift.max())
+ if self.wrap:
+ raw_cols = np.arange(lo, hi) % w
+ else:
+ lo = max(lo, 0)
+ hi = min(hi, w)
+ raw_cols = None
+
+ pts = pc.points
+
+ def cube(name: str) -> np.ndarray:
+ a = pts[name].reshape(w, e, n)
+ a = a[raw_cols] if raw_cols is not None else a[lo:hi]
+ return a.transpose(2, 0, 1)
+
+ x, y, z = cube("x"), cube("y"), cube("z")
+ good = (x != 0) | (y != 0) | (z != 0)
+ r = np.sqrt(x * x + y * y + z * z, dtype=np.float32)
+ good &= np.isfinite(r) # драйверы, отдающие «нет эха» как NaN
+ r = np.where(good, r, np.float32(0.0))
+
+ # Порядок: элевация каждой точки должна совпасть с элевацией её кольца.
+ # У «нет эха» x = y = z = r = 0, и отклонение тоже ноль.
+ dev = np.abs(z - r * self._sin_el[:, None, None])
+ if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
+ return None
+
+ if e == 1:
+ r_near = r[..., 0]
+ r_far = r[..., 0]
+ it = cube("intensity")[..., 0]
+ valid = good[..., 0]
+ else:
+ inten = cube("intensity")
+ near_i = np.argmin(np.where(good, r, np.float32(np.inf)), axis=-1)[..., None]
+ far_i = np.argmax(r, axis=-1)[..., None]
+ r_near = np.take_along_axis(r, near_i, -1)[..., 0]
+ r_far = np.take_along_axis(r, far_i, -1)[..., 0]
+ it = np.take_along_axis(inten, near_i, -1)[..., 0]
+ valid = good.any(axis=-1)
+
+ # выпрямление скоса каналов, с поправкой на смещение окна
+ g = self.gather[:, start:stop]
+ ok = self.gather_ok[:, start:stop]
+ if raw_cols is not None:
+ g = (g - lo) % w
+ else:
+ g = g - lo
+ ok = ok & (g >= 0) & (g < (hi - lo))
+ np.clip(g, 0, hi - lo - 1, out=g)
+
+ r_near = np.take_along_axis(r_near, g, 1)
+ r_far = np.take_along_axis(r_far, g, 1)
+ it = np.take_along_axis(it, g, 1)
+ valid = np.take_along_axis(valid, g, 1) & ok
+
+ r_near = np.where(valid, r_near, np.float32(0.0))
+ r_far = np.where(valid, r_far, np.float32(0.0))
+ return RangeImage(pc.stamp, np.ascontiguousarray(r_near),
+ np.ascontiguousarray(r_far), np.ascontiguousarray(it),
+ np.ascontiguousarray(valid))
+
+ def _project_geometric(self, pc: PointCloud2, start: int, stop: int) -> RangeImage:
+ """Раскладка по углам: кольцо — по элевации точки, столбец — по азимуту.
+
+ Не опирается на порядок точек вовсе. Если в ячейку попало несколько
+ точек (двойное эхо, вставленный предмет поверх фона), ближняя идёт в
+ `r_near`, дальняя — в `r_far`, как у целого кадра.
+ """
+ n, wid = self.n_rings, stop - start
+ pts = pc.points
+ x = np.asarray(pts["x"], np.float32)
+ y = np.asarray(pts["y"], np.float32)
+ z = np.asarray(pts["z"], np.float32)
+ good = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) \
+ & np.isfinite(z)
+ idx = np.flatnonzero(good)
+ x, y, z = x[idx], y[idx], z[idx]
+
+ # грубый отбор сектора по азимуту, без поправки кольца
+ step = self.az_step_deg
+ az = np.degrees(np.arctan2(x, -y))
+ jf = (az - np.float32(self.az0_deg)) / np.float32(step)
+ if self.wrap:
+ jf = np.mod(jf, self.n_az)
+ margin = float(np.abs(self.az_resid_deg).max()) / abs(step) + 1.0
+ sel = np.flatnonzero((jf > start - margin) & (jf < stop - 1 + margin))
+ idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]
+
+ r = np.sqrt(x * x + y * y + z * z)
+ el = np.degrees(np.arcsin(np.clip(z / np.maximum(r, np.float32(1e-6)), -1.0, 1.0)))
+ # ближайшее кольцо по элевации
+ k = np.clip(np.searchsorted(self._el_asc, el), 1, n - 1)
+ k -= (el - self._el_asc[k - 1]) < (self._el_asc[k] - el)
+ h = self._el_order[k]
+ ok = np.abs(el - self.el_deg[h]) <= np.maximum(self.el_step_deg[h], 0.2)
+ j = np.rint((az - self.az_resid_deg[h] - self.az0_deg) / step).astype(np.int64)
+ if self.wrap:
+ j %= self.n_az
+ ok &= (j >= start) & (j < stop)
+ sel = np.flatnonzero(ok)
+
+ r_near = np.zeros(n * wid, np.float32)
+ r_far = np.zeros(n * wid, np.float32)
+ it = np.zeros(n * wid, np.float32)
+ valid = np.zeros(n * wid, bool)
+ if sel.size:
+ cell = h[sel] * wid + (j[sel] - start)
+ rr = r[sel]
+ # сортировка по (ячейка, дальность): первая в ячейке — ближняя
+ mm = np.minimum(rr * 1000.0, (1 << 20) - 1).astype(np.int64)
+ order = np.argsort(cell * (1 << 20) + mm)
+ cs, rs = cell[order], rr[order]
+ brk = np.flatnonzero(cs[1:] != cs[:-1])
+ first = np.concatenate(([0], brk + 1))
+ last = np.concatenate((brk, [cs.size - 1]))
+ r_near[cs[first]] = rs[first]
+ r_far[cs[last]] = rs[last]
+ valid[cs[first]] = True
+ if "intensity" in pts.dtype.names:
+ src = np.asarray(pts["intensity"], np.float32)[idx[sel]][order]
+ it[cs[first]] = src[first]
+ return RangeImage(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
+ it.reshape(n, wid), valid.reshape(n, wid))
+
+ # ------------------------------------------------------------------ калибровка
+
+ @staticmethod
+ def calibrate(clouds: list[PointCloud2], n_rings: int | None = None) -> "ScanLayout":
+ """Восстановить решётку по нескольким кадрам.
+
+ Определяются: число колец и эх, элевация каждого кольца, шаг развёртки,
+ азимутальный сдвиг каждого канала и целочисленное выпрямление образа.
+ """
+ if not clouds:
+ raise ValueError("нужен хотя бы один кадр для калибровки")
+ pc0 = clouds[0]
+ if "ring" not in pc0.points.dtype.names:
+ # Без поля `ring` (синтетика организаторов): кольца — по гистограмме
+ # элевации, а в калибровку по порядку точек идут только кадры, где
+ # этот порядок цел. Нет таких — решётка строится целиком по углам.
+ el_ring = _ring_elevations(clouds)
+ if n_rings is None:
+ n_rings = el_ring.size
+ whole = [pc for pc in clouds if _is_organized(pc, el_ring)]
+ whole = [pc for pc in whole if pc.n_points == whole[0].n_points] if whole else []
+ if not whole:
+ return ScanLayout._calibrate_geometric(clouds, el_ring)
+ clouds, pc0 = whole, whole[0]
+ if n_rings is None:
+ n_rings = int(pc0.points["ring"].max()) + 1
+ n_cols, rem = divmod(pc0.n_points, n_rings)
+ if rem:
+ raise ValueError(f"{pc0.n_points} точек не делится на {n_rings} колец")
+
+ # Направление луча задано сенсором и в каждом кадре одно и то же:
+ # кадры нужны только чтобы закрыть лучи, не вернувшие эхо. Поэтому
+ # берётся первое конечное значение, а не медиана по стопке кадров —
+ # та стоила 1.4 с из 2.8 с всей калибровки и ничего не уточняла:
+ # ниже и шаг развёртки, и сдвиг канала берутся медианой по тысячам
+ # столбцов, где шум одного отсчёта всё равно усредняется.
+ az = el = None
+ for pc in clouds:
+ x = pc.points["x"].reshape(n_cols, n_rings).T.astype(np.float32)
+ y = pc.points["y"].reshape(n_cols, n_rings).T.astype(np.float32)
+ z = pc.points["z"].reshape(n_cols, n_rings).T.astype(np.float32)
+ ok = (x != 0) | (y != 0) | (z != 0)
+ with _quiet():
+ a = np.where(ok, np.degrees(np.arctan2(x, -y)), np.nan)
+ e = np.where(ok, np.degrees(np.arctan2(z, np.hypot(x, y))), np.nan)
+ if az is None:
+ az, el = a, e
+ continue
+ gap = np.isnan(az)
+ if not gap.any():
+ break
+ az[gap] = a[gap]
+ el[gap] = e[gap]
+ az = az.astype(np.float64)
+ el = el.astype(np.float64)
+
+ n_echo = _detect_echoes(az)
+ n_az = n_cols // n_echo
+ if n_echo > 1:
+ with _quiet():
+ az = np.nanmean(az.reshape(n_rings, n_az, n_echo), axis=2)
+ el = np.nanmean(el.reshape(n_rings, n_az, n_echo), axis=2)
+
+ # общий шаг развёртки: медиана по кольцам от робастной оценки наклона
+ slopes = []
+ for h in range(n_rings):
+ row = az[h]
+ idx = np.flatnonzero(np.isfinite(row))
+ if idx.size < 50:
+ continue
+ d = np.diff(np.unwrap(np.radians(row[idx]))) / np.diff(idx)
+ slopes.append(np.median(np.degrees(d)))
+ if not slopes:
+ raise ValueError("недостаточно валидных лучей для калибровки развёртки")
+ step = float(np.median(slopes))
+
+ # смещение каждого канала относительно общей развёртки
+ j = np.arange(n_az, dtype=np.float64)
+ base = step * j
+ with _quiet():
+ c_ring = np.nanmedian(_wrap180(az - base[None, :]), axis=1) # (H,)
+ c_ring = _fill_linear(c_ring)
+ c0 = float(np.median(c_ring))
+
+ # круговой скан: развёртка покрывает полные 360°
+ wrap = abs(step) * n_az > 350.0
+
+ # начало отсчёта выбирается так, чтобы «вперёд» (азимут 0) был в середине
+ # образа — иначе шов ±180° разрезал бы рабочий сектор пополам
+ if wrap:
+ k = int(np.rint(-c0 / step)) - n_az // 2
+ c0 = _wrap180(c0 + step * k)
+ # разница берётся по кратчайшей дуге: иначе шов ±180° даёт сдвиг в пол-оборота
+ shift = np.rint(_wrap180(c0 - c_ring) / step).astype(np.int64)
+ resid = _wrap180(c_ring + step * shift - c0)
+
+ with _quiet():
+ el_ring = _fill_linear(np.nanmedian(el, axis=1))
+ return ScanLayout(el_ring, step, float(c0), shift, resid, n_az, n_echo, wrap)
+
+ @staticmethod
+ def _calibrate_geometric(clouds: list[PointCloud2], el_ring: np.ndarray) -> "ScanLayout":
+ """Решётка только по углам точек, когда ни в одном кадре нет целого порядка.
+
+ Кольцо — ближайшая элевация из `el_ring`. Шаг развёртки — самая частая
+ разность соседних азимутов внутри кольца, сдвиг кольца внутри шага —
+ круговое среднее фазы его азимутов. Столбец растёт, азимут убывает — как
+ у Pandar128 в наших бэгах, чтобы образ не отразился зеркально.
+ """
+ n = el_ring.size
+ el_desc = np.sort(el_ring)[::-1]
+ order = np.argsort(el_desc)
+ asc = el_desc[order]
+ azs, hs = [], []
+ for pc in clouds[:6]:
+ x, y, z, ok = _xyz64(pc)
+ x, y, z = x[ok], y[ok], z[ok]
+ el = np.degrees(np.arctan2(z, np.hypot(x, y)))
+ k = np.clip(np.searchsorted(asc, el), 1, n - 1)
+ k -= (el - asc[k - 1]) < (asc[k] - el)
+ h = order[k]
+ on = np.abs(el - el_desc[h]) < ORDER_TOL_DEG
+ azs.append(np.degrees(np.arctan2(x[on], -y[on])))
+ hs.append(h[on])
+ az = np.concatenate(azs)
+ h = np.concatenate(hs)
+
+ diffs = []
+ for ring in range(n):
+ a = np.unique(np.round(az[h == ring], 4))
+ if a.size > 20:
+ diffs.append(np.diff(a))
+ if not diffs:
+ raise ValueError("недостаточно валидных лучей для калибровки развёртки")
+ d = np.concatenate(diffs)
+ d = d[d > 1e-3]
+ vals, cnt = np.unique(np.round(d, 3), return_counts=True)
+ step = float(vals[np.argmax(cnt)])
+ step = float(np.median(d[np.abs(d - step) < 0.1 * step]))
+
+ ph = np.exp(2j * np.pi * az / step)
+ s = (np.bincount(h, weights=ph.real, minlength=n)
+ + 1j * np.bincount(h, weights=ph.imag, minlength=n))
+ ref = float(np.angle(s.sum()) / (2 * np.pi) * step)
+ frac = np.where(np.abs(s) > 0, np.angle(s) / (2 * np.pi) * step, ref)
+ resid = (frac - ref + step / 2) % step - step / 2
+
+ lo, hi = np.percentile(az, [0.05, 99.95])
+ wrap = bool(hi - lo > 350.0)
+ if wrap:
+ n_az = int(round(360.0 / step))
+ az0 = ref + step * round((180.0 - ref) / step)
+ else:
+ az0 = ref + step * round((hi - ref) / step)
+ n_az = int(round((az0 - lo) / step)) + 1
+ return ScanLayout(el_desc, -step, float(az0), np.zeros(n, np.int64), resid,
+ n_az, 1, wrap, indexed=False)
+
+ # ------------------------------------------------------------------ сериализация
+
+ def save(self, path: str | Path) -> None:
+ np.savez_compressed(path, el_deg=self.el_deg, az_step_deg=self.az_step_deg,
+ az0_deg=self.az0_deg, col_shift=self.col_shift,
+ az_resid_deg=self.az_resid_deg, n_az=self.n_az,
+ n_echo=self.n_echo, wrap=self.wrap, indexed=self.indexed)
+
+ @staticmethod
+ def load(path: str | Path) -> "ScanLayout":
+ d = np.load(path)
+ return ScanLayout(d["el_deg"], float(d["az_step_deg"]), float(d["az0_deg"]),
+ d["col_shift"], d["az_resid_deg"], int(d["n_az"]),
+ int(d["n_echo"]), bool(d["wrap"]) if "wrap" in d else False,
+ bool(d["indexed"]) if "indexed" in d else True)
+
+ def __repr__(self) -> str:
+ kind = "" if self.indexed else ", по углам точек"
+ return (f"ScanLayout(колец={self.n_rings}, азимутов={self.n_az}, эх={self.n_echo}, "
+ f"сектор {self.az_grid_deg.min():.1f}°…{self.az_grid_deg.max():.1f}°, "
+ f"шаг {abs(self.az_step_deg):.3f}°, "
+ f"элевация {self.el_deg.min():.1f}°…{self.el_deg.max():.1f}°, "
+ f"скос каналов {np.ptp(self.col_shift)} стлб{kind})")
+
+
+# ---------------------------------------------------------------------- вспомогательное
+
+@contextmanager
+def _quiet():
+ """Пустые срезы и деление на ноль при калибровке — норма, а не ошибка."""
+ with warnings.catch_warnings(), np.errstate(all="ignore"):
+ warnings.simplefilter("ignore", RuntimeWarning)
+ yield
+
+
+def _wrap180(a):
+ """Привести угол(ы) в градусах к полуинтервалу (−180, 180]."""
+ return -((-np.asarray(a, np.float64) + 180.0) % 360.0 - 180.0)
+
+
+def _xyz64(pc: PointCloud2):
+ """Координаты в float64 и маска точек с эхом (не ноль и не NaN)."""
+ p = pc.points
+ x = p["x"].astype(np.float64)
+ y = p["y"].astype(np.float64)
+ z = p["z"].astype(np.float64)
+ ok = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) & np.isfinite(z)
+ return x, y, z, ok
+
+
+def _ring_elevations(clouds: list[PointCloud2], max_frames: int = 6) -> np.ndarray:
+ """Элевации колец по гистограмме, без поля `ring` и без опоры на порядок.
+
+ У лазерного канала элевация постоянна до 0.0001°, поэтому точки кольца
+ ложатся в один бин в 0.002°; соседние кольца Pandar128 разнесены на 0.086°
+ и больше. Разброс float32 может расщепить кольцо на соседние бины — они
+ сливаются. Вставленные точки (синтетика) рассыпаны по элевации и дают
+ мелкие группы, которые отсекает порог по весу. Порядок — сверху вниз,
+ как нумерует каналы Hesai.
+ """
+ parts = []
+ for pc in clouds[:max_frames]:
+ x, y, z, ok = _xyz64(pc)
+ parts.append(np.degrees(np.arctan2(z[ok], np.hypot(x[ok], y[ok]))))
+ el = np.concatenate(parts)
+ if el.size == 0:
+ raise ValueError("нет ни одной точки с эхом для калибровки колец")
+ q, cnt = np.unique(np.rint(el / 0.002).astype(np.int64), return_counts=True)
+ grp = np.concatenate(([0], np.cumsum(np.diff(q) > 5)))
+ w = np.bincount(grp, weights=cnt)
+ c = np.bincount(grp, weights=cnt * q * 0.002) / w
+ top = np.sort(w)[-min(64, w.size):]
+ keep = w >= 0.25 * np.median(top)
+ if keep.sum() < 2:
+ raise ValueError("не удалось выделить кольца по элевации")
+ return np.sort(c[keep])[::-1]
+
+
+def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
+ """Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
+ n = el_ring.size
+ if pc.n_points == 0 or pc.n_points % n:
+ return False
+ x, y, z, ok = _xyz64(pc)
+ el = np.degrees(np.arctan2(z, np.hypot(x, y))).reshape(-1, n)
+ ok = ok.reshape(-1, n)
+ for table in (el_ring, el_ring[::-1]):
+ if np.all(np.abs(el - table[None, :])[ok] < ORDER_TOL_DEG):
+ return True
+ return False
+
+
+def _detect_echoes(az: np.ndarray) -> int:
+ """Двойное эхо: соседние столбцы делят азимут (проверяется по каждому кольцу)."""
+ if az.shape[1] < 4:
+ return 1
+ a, b = az[:, 0::2], az[:, 1::2]
+ m = np.isfinite(a) & np.isfinite(b)
+ if m.sum() < 100:
+ return 1
+ return 2 if np.mean(np.abs(a[m] - b[m]) < 1e-3) > 0.8 else 1
+
+
+def _fill_linear(a: np.ndarray) -> np.ndarray:
+ """Линейно достроить NaN-пропуски по индексу (сетка равномерная)."""
+ a = np.asarray(a, np.float64).copy()
+ bad = ~np.isfinite(a)
+ if bad.all():
+ raise ValueError("нет ни одного валидного угла для калибровки")
+ if bad.any():
+ idx = np.arange(a.size)
+ a[bad] = np.interp(idx[bad], idx[~bad], a[~bad])
+ return a
diff --git a/ros2_ws/src/flyguard/flyguard/ros_conv.py b/ros2_ws/src/flyguard/flyguard/ros_conv.py
new file mode 100644
index 0000000..9feea83
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/ros_conv.py
@@ -0,0 +1,38 @@
+"""Преобразование sensor_msgs/PointCloud2 из rclpy во внутреннее представление.
+
+Внутри ноды сообщение уже разобрано транспортом, поэтому CDR-парсер не нужен:
+достаточно посмотреть на поля и построить структурированный numpy-массив
+поверх готового буфера, без копирования.
+"""
+from __future__ import annotations
+
+import numpy as np
+
+from .cdr import PointCloud2, point_dtype
+
+
+def from_ros(msg) -> PointCloud2:
+ """sensor_msgs.msg.PointCloud2 → flyguard.cdr.PointCloud2 (без копирования)."""
+ fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
+ fields.sort(key=lambda f: f[1])
+ dt = point_dtype(fields, msg.point_step)
+ buf = msg.data if isinstance(msg.data, (bytes, bytearray, memoryview)) else \
+ np.asarray(msg.data, np.uint8).tobytes()
+ n = msg.height * msg.width
+ pts = np.frombuffer(buf, dtype=dt, count=n)
+ 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=msg.is_dense, points=pts)
+
+
+def has_required_fields(msg) -> tuple[bool, str]:
+ """Проверить, что в облаке есть всё необходимое конвейеру."""
+ names = {f.name for f in msg.fields}
+ need = {"x", "y", "z", "intensity"}
+ missing = need - names
+ if missing:
+ return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
+ if msg.height * msg.width == 0:
+ return False, "пустое облако"
+ return True, ""
diff --git a/ros2_ws/src/flyguard/flyguard/synth.py b/ros2_ws/src/flyguard/flyguard/synth.py
new file mode 100644
index 0000000..0a42b90
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/synth.py
@@ -0,0 +1,418 @@
+"""Синтетические препятствия: трассировка лучей в реальные кадры.
+
+Разметки в датасете нет, а организаторы прямо предупредили, что приватный тест
+собран добавлением синтезированных препятствий в новые проезды. Поэтому свой
+полигон строится тем же способом: берётся настоящий кадр пустого тоннеля,
+в него трассировкой лучей вставляется предмет заданного размера на заданной
+дистанции, и получается **размеченный** пример с точно известным ответом.
+
+Вставка идёт в исходное облако точек, а не в готовый дальностный образ, поэтому
+через конвейер проходит ровно тот же путь, что и настоящие данные, начиная
+с ретины.
+
+Модель сенсора намеренно пессимистична: добавляется шум дальности, а
+вероятность несостоявшегося эха растёт с расстоянием и с углом падения. Лучше
+недооценить свой детектор, чем на защите обнаружить, что полигон был слишком
+добрым. Яркость вставки берётся из самой записи — из возвратов на тех же
+лучах: абсолютной шкалы интенсивности в этих данных нет, и любое назначенное
+число делает вставку узнаваемой (см. длинный комментарий ниже).
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+
+from .cdr import PointCloud2
+from .geometry import RailPlane
+from .retina import ScanLayout
+
+INF = np.float32(np.inf)
+
+
+# --------------------------------------------------------------------------- тела
+
+@dataclass
+class Box:
+ """Параллелепипед в координатах пути, стоящий на плоскости рельсов."""
+
+ length: float # вдоль пути, м
+ width: float # поперёк, м
+ height: float # вверх от рельса, м
+ h_base: float = 0.0
+
+ def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
+ lo = np.array([-self.length / 2, -self.width / 2, self.h_base], np.float32)
+ hi = np.array([self.length / 2, self.width / 2, self.h_base + self.height], np.float32)
+ t0 = np.full(od.shape, -INF, np.float32)
+ t1 = np.full(od.shape, INF, np.float32)
+ for o, d, a, b in ((od, dd, lo[0], hi[0]), (ou, du, lo[1], hi[1]),
+ (oh, dh, lo[2], hi[2])):
+ with np.errstate(divide="ignore", invalid="ignore"):
+ ta = (a - o) / d
+ tb = (b - o) / d
+ lo_t = np.where(d != 0, np.minimum(ta, tb), np.where((o >= a) & (o <= b), -INF, INF))
+ hi_t = np.where(d != 0, np.maximum(ta, tb), np.where((o >= a) & (o <= b), INF, -INF))
+ t0 = np.maximum(t0, lo_t)
+ t1 = np.minimum(t1, hi_t)
+ hit = (t1 >= np.maximum(t0, 0.0)) & np.isfinite(t0)
+ return np.where(hit, np.maximum(t0, 0.0), INF)
+
+
+@dataclass
+class Cylinder:
+ """Вертикальный цилиндр — человек, столб, ведро."""
+
+ radius: float
+ height: float
+ h_base: float = 0.0
+
+ def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
+ a = dd * dd + du * du
+ b = 2.0 * (od * dd + ou * du)
+ c = od * od + ou * ou - self.radius ** 2
+ disc = b * b - 4 * a * c
+ ok = (disc > 0) & (a > 1e-9)
+ sq = np.sqrt(np.where(ok, disc, 0.0))
+ with np.errstate(divide="ignore", invalid="ignore"):
+ t = (-b - sq) / (2 * a)
+ t2 = (-b + sq) / (2 * a)
+ t = np.where(t > 0, t, t2)
+ h = oh + t * dh
+ ok &= (t > 0) & (h >= self.h_base) & (h <= self.h_base + self.height)
+ return np.where(ok, t, INF)
+
+
+@dataclass
+class Sphere:
+ radius: float
+ h_centre: float
+
+ def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
+ oz = oh - self.h_centre
+ a = dd * dd + du * du + dh * dh
+ b = 2.0 * (od * dd + ou * du + oz * dh)
+ c = od * od + ou * ou + oz * oz - self.radius ** 2
+ disc = b * b - 4 * a * c
+ ok = disc > 0
+ sq = np.sqrt(np.where(ok, disc, 0.0))
+ with np.errstate(divide="ignore", invalid="ignore"):
+ t = (-b - sq) / (2 * a)
+ return np.where(ok & (t > 0), t, INF)
+
+
+@dataclass
+class ObjectModel:
+ """Предмет: набор тел плюс отражательные свойства."""
+
+ name: str
+ parts: list = field(default_factory=list)
+ reflectivity: float = 0.3 # 0…1, доля отражённого света
+
+ def intersect(self, od, ou, oh, dd, du, dh) -> np.ndarray:
+ t = np.full(od.shape, INF, np.float32)
+ for p in self.parts:
+ t = np.minimum(t, p.intersect(od, ou, oh, dd, du, dh))
+ return t
+
+ @property
+ def size(self) -> tuple[float, float]:
+ """Грубые габариты (ширина, высота) для отчётов."""
+ w = h = 0.0
+ for p in self.parts:
+ if isinstance(p, Box):
+ w = max(w, p.width); h = max(h, p.h_base + p.height)
+ elif isinstance(p, Cylinder):
+ w = max(w, 2 * p.radius); h = max(h, p.h_base + p.height)
+ elif isinstance(p, Sphere):
+ w = max(w, 2 * p.radius); h = max(h, p.h_centre + p.radius)
+ return w, h
+
+
+def catalogue() -> dict[str, ObjectModel]:
+ """Набор предметов, встречающихся в тоннеле, от крупных к мелким."""
+ return {
+ "человек_стоя": ObjectModel("человек_стоя", [
+ Cylinder(radius=0.22, height=1.45),
+ Sphere(radius=0.11, h_centre=1.60)], reflectivity=0.35),
+ "человек_сидя": ObjectModel("человек_сидя", [
+ Box(0.45, 0.50, 0.85)], reflectivity=0.35),
+ "человек_лежа": ObjectModel("человек_лежа", [
+ Box(length=1.80, width=0.50, height=0.30, h_base=0.0)], reflectivity=0.35),
+ "ящик": ObjectModel("ящик", [Box(0.60, 0.60, 0.60)], reflectivity=0.40),
+ "чемодан": ObjectModel("чемодан", [Box(0.25, 0.45, 0.55)], reflectivity=0.30),
+ "ведро": ObjectModel("ведро", [Cylinder(radius=0.15, height=0.35)], reflectivity=0.45),
+ "камень": ObjectModel("камень", [Sphere(radius=0.11, h_centre=0.11)], reflectivity=0.20),
+ "бутылка": ObjectModel("бутылка", [Cylinder(radius=0.045, height=0.30)],
+ reflectivity=0.25),
+ "кабель": ObjectModel("кабель", [Box(2.20, 0.06, 0.06)], reflectivity=0.15),
+ "каска": ObjectModel("каска", [Sphere(radius=0.14, h_centre=0.10)], reflectivity=0.55),
+ }
+
+
+# --------------------------------------------------------------------------- вставка
+
+@dataclass
+class Placement:
+ """Где стоит предмет."""
+
+ d: float # вперёд от сенсора, м
+ u: float = 0.0 # поперёк от оси пути, м
+ yaw_deg: float = 0.0 # поворот вокруг вертикали (для вытянутых тел)
+
+
+def ray_dirs_track(layout: ScanLayout, plane: RailPlane):
+ """Направления лучей в координатах пути: (dd, du, dh) и начало (0, 0, H)."""
+ dirs = layout.dirs
+ dd = -dirs[..., 1].astype(np.float32)
+ du = dirs[..., 0].astype(np.float32)
+ dh = (dirs[..., 2] - plane.a * dd - plane.b * du).astype(np.float32)
+ return dd, du, dh, np.float32(plane.height)
+
+
+# Паспортные данные Pandar128E3X (руководство v4p5, п. 1.4 и Приложение A):
+# дальность 0.3…200 м при отражательной способности 10 %, вероятность
+# обнаружения на паспортной дальности PoD = 70 %, точность ±2 см на 1…200 м.
+SPEC_REFLECTIVITY = 0.10
+SPEC_POD = 0.70
+HARD_MAX_RANGE = 230.0 # дальше в датасете возвратов не встречается
+
+# Расходимость луча в руководстве не указана; принята равной угловому шагу
+# решётки (0.1° по азимуту, 0.125° по элевации в полосе высокого разрешения) —
+# это верхняя оценка, дающая консервативный результат для мелких предметов.
+BEAM_DIV_H = 1.75e-3 # рад
+BEAM_DIV_V = 2.18e-3 # рад
+
+_CHANNEL_RANGE: np.ndarray | None = None
+
+# Интенсивность вставки. Здесь нельзя придумать ни формулы, ни числа.
+#
+# Сначала стояла ламбертова ρ·cosθ/r². Прибор, однако, отдаёт не принятую
+# энергию, а отражательную способность с компенсацией дальности: медиана по
+# облаку держится 8…9 от 5 до 50 м и как 1/r² не падает. Вставка получалась на
+# 55 м в двадцать раз тусклее настоящего предмета, а за 110 м упиралась в
+# нижний срез шкалы — и «тускло» становилось безошибочным признаком предмета.
+#
+# Замена на постоянную яркость 100·ρ·√cosθ, привязанную к настоящему предмету
+# (34.5 на 55.9 м), просто перевернула артефакт: 35 против 3…7 у обстановки на
+# каждой полосе дальности, AUC по одной интенсивности 0.96…0.97.
+#
+# Причина в том, что абсолютной шкалы тут нет. Медиана яркости кандидатов
+# обстановки по бэгам: 3.0, 4.0, 4.0, 4.0, 7.0 — а в `doubleT_obstacle`, где
+# лежит настоящий предмет, 23.5 при 34.5 у самого предмета. Разница между
+# записями впятеро больше, чем контраст предмета к фону внутри записи. Любое
+# абсолютное число, назначенное вставке, оказывается подарком детектору — в ту
+# или в другую сторону.
+#
+# Поэтому вставка берёт яркость **реальных возвратов с тех же самых лучей** —
+# того, что предмет заслонил; где эха не было, из возвратов вдоль остальных его
+# лучей. Признак становится неинформативным, и полигон меряет геометрию и
+# движение, то есть то, что мы моделируем честно. Оценка заниженная: настоящий
+# предмет в записи был в 1.47 раза ярче окружения. На линии с откалиброванной
+# яркостью этот запас можно вернуть — замером, а не верой.
+#
+# Отражательная способность никуда не делась: она определяет, вернётся ли эхо
+# вообще (`dropout_probability`), а это и есть её настоящая роль.
+INTEN_SPREAD = 0.12 # разброс отсчёта, логнормальный, ≈ ±12 %
+
+
+class IntensityEnv:
+ """Яркость реальных возвратов кадра, разложенная по дальности.
+
+ Нужна, чтобы луч предмета, ушедший в пустоту, получил яркость такую же, как
+ у настоящих возвратов С ТОЙ ЖЕ дальности, а не с ближних и ярких. Считается
+ один раз на кадр и переиспользуется всеми сценариями: в сборе выборки их
+ 135 на кадр, и пересчитывать корни по полутора миллионам точек для каждого
+ незачем.
+ """
+
+ EDGES = np.array([0, 20, 40, 60, 90, 130, 180, 260], np.float32)
+
+ def __init__(self, pc: PointCloud2):
+ q = pc.points
+ it = np.asarray(q["intensity"], np.float32)
+ x, y, z = q["x"], q["y"], q["z"]
+ r2 = x * x + y * y + z * z
+ ok = (it > 0) & (r2 > 1.0) & np.isfinite(r2)
+ r = np.sqrt(r2[ok], dtype=np.float32)
+ v = it[ok]
+ b = np.clip(np.searchsorted(self.EDGES, r, side="right") - 1,
+ 0, self.EDGES.size - 2)
+ order = np.argsort(b, kind="stable")
+ b_s, v_s = b[order], v[order]
+ cut = np.searchsorted(b_s, np.arange(self.EDGES.size - 1), side="left")
+ cut = np.append(cut, b_s.size)
+ self._pools = [v_s[cut[i]:cut[i + 1]] for i in range(self.EDGES.size - 1)]
+ self._all = v_s
+
+ def sample(self, d: float, n: int, rng: np.random.Generator) -> np.ndarray:
+ i = int(np.clip(np.searchsorted(self.EDGES, d, side="right") - 1,
+ 0, self.EDGES.size - 2))
+ pool = self._pools[i]
+ if pool.size < 32: # на этой дальности возвратов нет
+ pool = self._all
+ if pool.size == 0:
+ return np.full(n, 5.0, np.float32)
+ return rng.choice(pool, size=n).astype(np.float32)
+
+
+def local_intensity(prev: np.ndarray, pool: np.ndarray, frame: np.ndarray,
+ rng: np.random.Generator, env: "IntensityEnv | None" = None,
+ d: float = 0.0) -> np.ndarray:
+ """Яркость вставки по окружению. Отражения среди аргументов нет намеренно.
+
+ Дальность участвует ровно в одном качестве — какую полосу реальных
+ возвратов брать для лучей, ушедших в пустоту. Никакого закона яркости от
+ дальности здесь не задаётся.
+ """
+ v = np.asarray(prev, np.float32).copy()
+ miss = ~(v > 0)
+ if miss.any():
+ n = int(miss.sum())
+ if env is not None:
+ v[miss] = env.sample(d, n, rng)
+ else:
+ src = pool[pool > 0] if pool.size else np.empty(0, np.float32)
+ if src.size == 0: # все лучи предмета в пустоту
+ wide = frame[::997]
+ src = wide[wide > 0]
+ v[miss] = (rng.choice(src, size=n) if src.size else np.float32(5.0))
+ v = v * rng.lognormal(0.0, INTEN_SPREAD, v.shape)
+ return np.clip(v, 1.0, 255.0).astype(np.float32)
+
+
+def channel_max_range() -> np.ndarray:
+ """Паспортная дальность каждого канала при 10 % отражения, (128,).
+
+ Каналы сильно неравноправны: 34–65 «дальнобойные» и берут 200 м, а 98–128
+ смотрят в землю и рассчитаны только на ближнее и среднее поле. Без учёта
+ этого синтетический полигон завышал бы дальность обнаружения для предметов,
+ попадающих в нижние каналы.
+ """
+ global _CHANNEL_RANGE
+ if _CHANNEL_RANGE is None:
+ import csv
+ from pathlib import Path
+ path = Path(__file__).with_name("data") / "pandar128_channels.csv"
+ try:
+ rows = sorted(csv.DictReader(open(path, encoding="utf-8")),
+ key=lambda r: int(r["channel"]))
+ _CHANNEL_RANGE = np.array([float(r["max_range_10pct_m"]) for r in rows],
+ np.float32)
+ except (OSError, KeyError, ValueError):
+ _CHANNEL_RANGE = np.full(128, 200.0, np.float32)
+ return _CHANNEL_RANGE
+
+
+def dropout_probability(r: np.ndarray, reflectivity: float,
+ ang_w: np.ndarray | float = 1.0,
+ ang_h: np.ndarray | float = 1.0,
+ max_range: np.ndarray | float = 200.0) -> np.ndarray:
+ """Вероятность, что эхо не вернётся.
+
+ Принятая мощность падает как ρ·A/r², где A — доля пятна луча, закрытая
+ предметом. Отсюда «эффективная дальность» r·√(ρ_паспорт/(ρ·A)), которую
+ остаётся сравнить с паспортной дальностью **этого канала**. Переход сделан
+ логистическим и смещён так, чтобы на паспортной дальности вероятность
+ обнаружения равнялась заявленным 70 %.
+
+ Заполнение пятна важно именно для мелких предметов: на 200 м луч шириной
+ 1.75 мрад покрывает 35 см, и бутылка диаметром 9 см отражает лишь четверть
+ его энергии — поэтому она пропадает намного раньше человека, хотя по
+ геометрии в неё ещё попадают лучи.
+ """
+ fill = np.clip(ang_w / BEAM_DIV_H, 0.05, 1.0) * np.clip(ang_h / BEAM_DIV_V, 0.05, 1.0)
+ rho = max(reflectivity, 0.02) * fill
+ eff = r * np.sqrt(SPEC_REFLECTIVITY / rho)
+ width = 0.12 * np.asarray(max_range, np.float32)
+ r50 = np.asarray(max_range, np.float32) + width * np.log(SPEC_POD / (1 - SPEC_POD))
+ p_detect = 1.0 / (1.0 + np.exp((eff - r50) / np.maximum(width, 1e-3)))
+ p_detect = np.where(r > HARD_MAX_RANGE, 0.0, p_detect)
+ return np.clip(1.0 - p_detect, 0.0, 1.0).astype(np.float32)
+
+
+def inject(pc: PointCloud2, layout: ScanLayout, plane: RailPlane,
+ obj: ObjectModel, place: Placement, *,
+ rng: np.random.Generator | None = None,
+ range_noise: float = 0.02, cols: slice | None = None,
+ env: "IntensityEnv | None" = None) -> tuple[PointCloud2, dict]:
+ """Вставить предмет в облако точек. Возвращает (новое облако, разметка)."""
+ rng = rng or np.random.default_rng()
+ n_rings, n_az, n_echo = layout.n_rings, layout.n_az, layout.n_echo
+
+ dd, du, dh, H = ray_dirs_track(layout, plane)
+ # начало луча в системе предмета: сенсор в (0,0,H), предмет в (d, u, 0)
+ od = np.full(dd.shape, -np.float32(place.d), np.float32)
+ ou = np.full(dd.shape, -np.float32(place.u), np.float32)
+ oh = np.full(dd.shape, H, np.float32)
+
+ if place.yaw_deg:
+ c, s = np.cos(np.radians(place.yaw_deg)), np.sin(np.radians(place.yaw_deg))
+ od, ou = c * od + s * ou, -s * od + c * ou
+ dd, du = c * dd + s * du, -s * dd + c * du
+
+ t = obj.intersect(od, ou, oh, dd, du, dh)
+ hit = np.isfinite(t) & (t > 1.0)
+ if not hit.any():
+ return pc, dict(hit_rays=0, d=place.d, u=place.u, name=obj.name)
+
+ # шум дальности и пропуски эха
+ t = t + rng.normal(0.0, range_noise, t.shape).astype(np.float32)
+ w_obj, h_obj = obj.size
+ ang_w = w_obj / np.maximum(t, 1.0)
+ ang_h = h_obj / np.maximum(t, 1.0)
+ ch_range = channel_max_range()
+ per_ray_range = (ch_range[:n_rings, None] if ch_range.size >= n_rings
+ else np.float32(200.0))
+ p_drop = dropout_probability(t, obj.reflectivity, ang_w, ang_h, per_ray_range)
+ hit &= rng.random(t.shape) > p_drop
+
+ pts = pc.points.copy()
+ ring_i, col_i = np.nonzero(hit)
+ raw_col = layout.gather[ring_i, col_i] # выпрямленный столбец → сырой
+ new_r = t[ring_i, col_i]
+
+ # плоский индекс точки: (сырой столбец · число эх + эхо) · число колец + кольцо
+ base = (raw_col * n_echo) * n_rings + ring_i
+ fx, fy, fz = pts["x"], pts["y"], pts["z"] # виды на поля, запись идёт в pts
+
+ # предмет виден, только если он ближе уже зарегистрированного эха
+ r_exist = np.full(base.shape, INF, np.float32)
+ for e in range(n_echo):
+ idx = base + e * n_rings
+ ex, ey, ez = fx[idx], fy[idx], fz[idx]
+ have = (ex != 0) | (ey != 0) | (ez != 0)
+ r_e = np.where(have, np.sqrt(ex * ex + ey * ey + ez * ez), INF)
+ r_exist = np.minimum(r_exist, r_e)
+
+ closer = new_r < r_exist
+ # яркость того, что было на лучах предмета: на выбранных — то, что он
+ # заслонил, остальные идут в запасной набор для лучей без эха
+ prev_sel = pts["intensity"][base[closer]]
+ pool = pts["intensity"][base]
+ ring_i, col_i, base, new_r = ring_i[closer], col_i[closer], base[closer], new_r[closer]
+ n_written = int(base.size)
+ if n_written:
+ dir_sel = layout.dirs[ring_i, col_i]
+ nx = (dir_sel[:, 0] * new_r).astype(np.float32)
+ ny = (dir_sel[:, 1] * new_r).astype(np.float32)
+ nz = (dir_sel[:, 2] * new_r).astype(np.float32)
+ inten = local_intensity(prev_sel, pool, pts["intensity"], rng,
+ env=env, d=place.d)
+ for e in range(n_echo):
+ idx = base + e * n_rings
+ fx[idx] = nx
+ fy[idx] = ny
+ fz[idx] = nz
+ pts["intensity"][idx] = inten.astype(np.float32)
+
+ out = PointCloud2(stamp=pc.stamp, frame_id=pc.frame_id, height=pc.height,
+ width=pc.width, point_step=pc.point_step,
+ is_dense=pc.is_dense, points=pts)
+ w, hgt = obj.size
+ return out, dict(hit_rays=int(n_written), d=float(place.d), u=float(place.u),
+ name=obj.name, width=w, height=hgt,
+ reflectivity=obj.reflectivity,
+ # индексы лучей, в которые предмет реально записан: по ним
+ # разметка кандидата точная, а не «по дальности примерно»
+ rays=(ring_i, col_i))
diff --git a/ros2_ws/src/flyguard/flyguard/track_readout.py b/ros2_ws/src/flyguard/flyguard/track_readout.py
new file mode 100644
index 0000000..ce0860c
--- /dev/null
+++ b/ros2_ws/src/flyguard/flyguard/track_readout.py
@@ -0,0 +1,121 @@
+"""Считывание по ТРЕКУ — тот же контур MBON, но на входе накопленная история.
+
+Считывание в `mbon_readout.py` смотрит на один кадр: контур кандидата, его
+тень, яркость, опору накопителя. Этого хватает вблизи и не хватает вдали —
+за 120 м предмет даёт пять-шесть лучей, и по одному кадру он неотличим от
+куска стены, попавшего в габарит.
+
+Но у трека есть то, чего у кандидата нет в принципе: **история**. Поезд
+проехал двадцать метров, а предмет, если он настоящий, остался на том же
+месте в тоннеле. Воронка потерь (EXPERIMENTS п. 12) показывает, что именно
+здесь и теряется дальнее обнаружение: на 120…160 м трек заводится в 62 %
+наблюдений, а до решения доходит 38 % — улика есть, порога не набирает.
+
+Поэтому вход здесь другой: сводки за всю жизнь трека, а не последнее
+значение, — средний и максимальный вес наблюдения, средний и максимальный
+покадровый отсчёт MBON, наклон числа лучей по времени, разбросы положения.
+
+**ЧЕМ ЭТО КОНЧИЛОСЬ.** Обученная модель не обгоняет ни один из признаков,
+которые ей же и скормили: leave-one-bag-out AUC 0.873 против 0.880 у одного
+среднего веса наблюдения и 0.895 у среднего отсчёта считывания. Причина — не в
+схеме: наблюдений 9 299, но это сотни кадров подряд по одному и тому же треку,
+и независимых примеров тут сотни, а не тысячи. Поэтому в решении участвует
+**не модель, а прямо среднее** (`descending.track_score`, по умолчанию
+`w_mean`), а всё в этом файле — инструмент замера. Разбор — EXPERIMENTS п. 15.
+
+Отдельно: `s_std` и `u_std` в решении использовать нельзя. На полигоне разброс
+привязки к месту выглядит лучшим признаком дальней полосы (AUC 0.869 за 160 м),
+а на настоящем объекте он перевёрнут (0.223): вставка ставится на дальность,
+посчитанную из той же оценки собственного движения, что двигает мировую
+координату, и разброса у неё нет по построению. Признаки оставлены в
+дескрипторе нарочно — чтобы проверку можно было повторить (п. 15.4).
+"""
+from __future__ import annotations
+
+import numpy as np
+
+from .central_complex import Track, expected_rays
+from .mbon_readout import MbonConfig, MbonReadout
+
+TRACK_FEATURES = (
+ "log_d", # дальность сейчас
+ "evidence", # накопленная улика — то, чем решают сегодня
+ "hits", # подтверждений
+ "age", # возраст, кадров
+ "hit_rate", # подтверждений на кадр жизни
+ "misses", # промахов подряд
+ "closed", # log(первая дальность / нынешняя): насколько подъехали
+ "novelty", # сглаженная новизна из памяти тоннеля
+ "width", "height", "h",
+ "speed_lat", # модуль поперечной скорости
+ "rays_now", # лучей к ожидаемым для этой дальности
+ "rays_mean", # то же, в среднем за жизнь
+ "rays_slope", # и наклон по времени
+ "w_mean", "w_max", # вес наблюдения
+ "p_mean", "p_max", # покадровый отсчёт считывания
+ # Оба разброса — ТОЛЬКО для замеров: на синтетике сильны, на реальном
+ # объекте перевёрнуты. См. шапку файла и EXPERIMENTS п. 15.4.
+ "s_std", # разброс места в тоннеле, м
+ "u_std", # разброс поперечного положения, м
+)
+
+
+def _std(n: int, s: float, sq: float) -> float:
+ """Разброс по накопленным суммам. Отрицательную дисперсию даёт
+ округление, а не данные, поэтому она просто отсекается."""
+ if n < 2:
+ return 0.0
+ var = sq / n - (s / n) ** 2
+ return float(np.sqrt(var)) if var > 0.0 else 0.0
+
+
+def describe_track(t: Track, s_now: float) -> np.ndarray:
+ """Вектор трека для считывания. Порядок — как в `TRACK_FEATURES`."""
+ d = max(t.distance(s_now), 1.0)
+ n = max(t.n_obs, 1)
+ den = t.n_obs * t.i_sq - t.i_sum * t.i_sum
+ slope = ((t.n_obs * t.ri_sum - t.i_sum * t.r_sum) / den
+ if t.n_obs > 1 and abs(den) > 1e-9 else 0.0)
+ return np.array((
+ np.log(d),
+ t.evidence,
+ float(t.hits),
+ float(t.age),
+ t.hits / max(t.age, 1),
+ float(t.misses),
+ np.log(max(t.first_d, 1.0) / d),
+ t.novelty,
+ t.width, t.height, t.h,
+ abs(t.speed_lat),
+ t.last_n_rays / expected_rays(d),
+ t.r_sum / n,
+ slope,
+ t.w_sum / n, t.w_max,
+ t.p_sum / n, t.p_max,
+ _std(t.n_obs, t.s_sum, t.s_sq),
+ _std(t.n_obs, t.u_sum, t.u_sq),
+ ), np.float32)
+
+
+class TrackReadout(MbonReadout):
+ """Тот же разрежённый код, другой вход и другой учитель.
+
+ Ёмкость меньше, чем у покадрового считывания: примеров тут на порядок
+ меньше (треков, а не кандидатов), и восемь тысяч клеток на такой выборке
+ заучивают её наизусть. Значение выбирается развёрткой в
+ `tools/train_track.py`, а не на глаз.
+ """
+
+ def __init__(self, cfg: MbonConfig | None = None,
+ n_pn: int = len(TRACK_FEATURES)):
+ super().__init__(cfg or MbonConfig(n_kc=2_000, seed=20260922),
+ n_pn=n_pn)
+
+ def annotate_tracks(self, tracks: list[Track], s_now: float) -> None:
+ """Проставить каждому треку отсчёт в `extra`-поле `p_track`."""
+ if not tracks:
+ return
+ X = np.stack([describe_track(t, s_now) for t in tracks])
+ p = 1.0 / (1.0 + np.exp(-self.logit(X)))
+ for t, pp in zip(tracks, p):
+ t.p_track = float(pp)
diff --git a/ros2_ws/src/flyguard/launch/detect.launch.py b/ros2_ws/src/flyguard/launch/detect.launch.py
new file mode 100644
index 0000000..5ed26fe
--- /dev/null
+++ b/ros2_ws/src/flyguard/launch/detect.launch.py
@@ -0,0 +1,150 @@
+"""Запуск детектора FlyGuard.
+
+ ros2 launch flyguard detect.launch.py
+ ros2 launch flyguard detect.launch.py rviz:=true brain_view:=true
+ ros2 launch flyguard detect.launch.py bag:=/data/for_hackathon/doubleT_obstacle
+ ros2 launch flyguard detect.launch.py bag:=/data/squareT_platform_squareT_switch start:=18
+
+Если указан `bag`, launch сам поднимет `ros2 bag play` — тогда для демонстрации
+достаточно одной команды; `start` — с какой секунды записи начать.
+"""
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, OpaqueFunction,
+ RegisterEventHandler, TimerAction)
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessIO
+from launch.substitutions import (LaunchConfiguration, PathJoinSubstitution,
+ PythonExpression)
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ share = Path(get_package_share_directory("flyguard"))
+ default_memory = share / "artifacts" / "mushroom_body.npz"
+ default_mbon = share / "artifacts" / "mbon_readout.npz"
+
+ args = [
+ DeclareLaunchArgument("input_topic", default_value="/lidar_points",
+ description="топик с облаком точек лидара"),
+ DeclareLaunchArgument("memory_path",
+ default_value=str(default_memory) if default_memory.exists() else "",
+ description="обученная память тоннеля (.npz)"),
+ DeclareLaunchArgument("mbon_path",
+ default_value=str(default_mbon) if default_mbon.exists() else "",
+ description="обученное считывание MBON (.npz); пусто — ручная формула"),
+ DeclareLaunchArgument("fov_deg", default_value="30.0"),
+ DeclareLaunchArgument("half_width", default_value="1.2",
+ description="полуширина габарита, м"),
+ DeclareLaunchArgument("best_effort", default_value="false",
+ description="QoS подписки: true — BEST_EFFORT, false — RELIABLE. "
+ "Кадры лидара крупные (до 24 МБ), и при BEST_EFFORT "
+ "потеря одного UDP-фрагмента отбрасывает всё сообщение"),
+ DeclareLaunchArgument("queue_depth", default_value="10",
+ description="глубина очереди подписки"),
+ DeclareLaunchArgument("async_worker", default_value="false",
+ description="обрабатывать в отдельном потоке; по умолчанию "
+ "в колбэке — так не теряется выработка на GIL"),
+ DeclareLaunchArgument("brain_view", default_value="false",
+ description="публиковать схему мозга мухи с активностью"),
+ DeclareLaunchArgument("brain_scale", default_value="1",
+ description="масштаб вида мозга: 1 — 1180 пикселей, "
+ "2–3 — для экрана и видео в 2K/4K"),
+ DeclareLaunchArgument("debug_cloud", default_value="false"),
+ DeclareLaunchArgument("rviz", default_value="false"),
+ DeclareLaunchArgument("bag", default_value="",
+ description="путь к bag-файлу; пусто — не запускать проигрывание"),
+ DeclareLaunchArgument("rate", default_value="1.0",
+ description="скорость проигрывания bag"),
+ DeclareLaunchArgument("start", default_value="0.0",
+ description="с какой секунды записи начать; узлу нужно "
+ "около двух секунд на калибровку и разгон"),
+ DeclareLaunchArgument("device", default_value="auto",
+ description="где считать сетчатку, ламину и кластеризацию: "
+ "auto — видеокарта, если есть (контейнер с --gpus all), "
+ "иначе процессор; cuda; cpu. Результат одинаков"),
+ DeclareLaunchArgument("log_level", default_value="info"),
+ ]
+
+ detector = Node(
+ package="flyguard",
+ executable="detector",
+ name="flyguard",
+ output="screen",
+ # BLAS в один поток: иначе потоки OpenBLAS крутятся вхолостую, и узел
+ # занимает 3 ядра вместо 0.4 при том же времени кадра (docker/Dockerfile)
+ additional_env={"OPENBLAS_NUM_THREADS": "1", "OMP_NUM_THREADS": "1",
+ "MKL_NUM_THREADS": "1"},
+ arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")],
+ parameters=[
+ PathJoinSubstitution([str(share), "config", "flyguard.yaml"]),
+ {
+ "input_topic": LaunchConfiguration("input_topic"),
+ "memory_path": LaunchConfiguration("memory_path"),
+ "mbon_path": LaunchConfiguration("mbon_path"),
+ "fov_deg": LaunchConfiguration("fov_deg"),
+ "half_width": LaunchConfiguration("half_width"),
+ "best_effort": LaunchConfiguration("best_effort"),
+ "queue_depth": LaunchConfiguration("queue_depth"),
+ "async_worker": LaunchConfiguration("async_worker"),
+ "brain_view": LaunchConfiguration("brain_view"),
+ "brain_scale": LaunchConfiguration("brain_scale"),
+ "publish_debug_cloud": LaunchConfiguration("debug_cloud"),
+ "device": LaunchConfiguration("device"),
+ },
+ ],
+ )
+
+ # Со схемой мозга — свой конфиг: схема справа во всю высоту. Без неё пустая
+ # панель картинки отъедала бы у трёхмерного вида треть окна.
+ rviz_cfg = PathJoinSubstitution([str(share / "config"), PythonExpression([
+ "'flyguard_brain.rviz' if '", LaunchConfiguration("brain_view"),
+ "'.lower() in ('true', '1') else 'flyguard.rviz'"])])
+ rviz = Node(
+ package="rviz2", executable="rviz2", name="rviz2", output="log",
+ condition=IfCondition(LaunchConfiguration("rviz")),
+ arguments=["-d", rviz_cfg],
+ )
+
+ # --read-ahead-queue-size 10: по умолчанию проигрыватель сперва читает
+ # вперёд 1000 сообщений — при кадре в 24 МБ это 24 ГБ, и пока он читает,
+ # первые секунды записи «просрочиваются» и не публикуются вовсе (замерено:
+ # 119 кадров из 201 без флага).
+ # --clock: время записи в /clock для пульта (flyguard-keys: пауза и
+ # перемотка). Узел и RViz живут по своим часам и его не слушают.
+ play = ExecuteProcess(
+ cmd=["ros2", "bag", "play", LaunchConfiguration("bag"),
+ "--rate", LaunchConfiguration("rate"), "--read-ahead-queue-size", "10",
+ "--start-offset", LaunchConfiguration("start"), "--clock", "20"],
+ output="screen",
+ # пустой аргумент `bag` означает «не запускать проигрывание»
+ condition=IfCondition(PythonExpression(["'", LaunchConfiguration("bag"), "' != ''"])),
+ )
+
+ # Проигрывание стартует, когда узел подписался: без этого первые кадры
+ # записи уходят в пустоту (замерено: 244 кадра из 252 против 252 из 252 у
+ # раздельного запуска). Фиксированной паузы мало: с видеокартой узел
+ # поднимается дольше — импорт PyTorch, контекст CUDA, прогрев. Поэтому ждём
+ # строку журнала «подписка на» и ещё 1.5 с на обнаружение DDS; не пришла за
+ # 30 с — запускаем всё равно.
+ started = {"play": False}
+
+ def start_play():
+ if started["play"]:
+ return []
+ started["play"] = True
+ return [play]
+
+ def on_output(event):
+ if not started["play"] and "подписка на" in event.text.decode("utf-8", "ignore"):
+ return [TimerAction(period=1.5, actions=[OpaqueFunction(
+ function=lambda context: start_play())])]
+ return None
+
+ wait_node = RegisterEventHandler(OnProcessIO(target_action=detector, on_stdout=on_output,
+ on_stderr=on_output))
+ fallback = TimerAction(period=30.0, actions=[OpaqueFunction(
+ function=lambda context: start_play())])
+ return LaunchDescription(args + [detector, rviz, wait_node, fallback])
diff --git a/ros2_ws/src/flyguard/package.xml b/ros2_ws/src/flyguard/package.xml
new file mode 100644
index 0000000..6cc498a
--- /dev/null
+++ b/ros2_ws/src/flyguard/package.xml
@@ -0,0 +1,35 @@
+
+
+
+ flyguard
+ 1.0.0
+
+ Обнаружение посторонних объектов в габарите беспилотного поезда по данным 3D-лидара.
+ Архитектура повторяет вычислительные схемы зрительной системы и грибовидных тел
+ Drosophila melanogaster, взятые из коннектома.
+
+ FlyGuard
+ MIT
+
+ rclpy
+ sensor_msgs
+ std_msgs
+ visualization_msgs
+ diagnostic_msgs
+ geometry_msgs
+ tf2_ros
+ flyguard_msgs
+ rosbag2_interfaces
+ rosgraph_msgs
+ builtin_interfaces
+ python3-numpy
+ python3-scipy
+
+ ament_copyright
+ ament_flake8
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/ros2_ws/src/flyguard/resource/flyguard b/ros2_ws/src/flyguard/resource/flyguard
new file mode 100644
index 0000000..e69de29
diff --git a/ros2_ws/src/flyguard/setup.cfg b/ros2_ws/src/flyguard/setup.cfg
new file mode 100644
index 0000000..068fcf4
--- /dev/null
+++ b/ros2_ws/src/flyguard/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/flyguard
+[install]
+install_scripts=$base/lib/flyguard
diff --git a/ros2_ws/src/flyguard/setup.py b/ros2_ws/src/flyguard/setup.py
new file mode 100644
index 0000000..704d555
--- /dev/null
+++ b/ros2_ws/src/flyguard/setup.py
@@ -0,0 +1,35 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "flyguard"
+
+setup(
+ name=package_name,
+ version="1.0.0",
+ packages=find_packages(exclude=["test"]),
+ data_files=[
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
+ ("share/" + package_name + "/config", glob("config/*")),
+ ("share/" + package_name + "/artifacts", glob("artifacts/*")),
+ ],
+ # Таблица каналов лидара и атлас нейронов лежат внутри пакета: без этого
+ # в образ попадают только .py, и вид мозга молча остаётся без данных.
+ package_data={package_name: ["data/*"]},
+ include_package_data=True,
+ install_requires=["setuptools"],
+ zip_safe=True,
+ maintainer="FlyGuard",
+ maintainer_email="team@flyguard.local",
+ description="Обнаружение посторонних объектов в тоннеле метро по данным 3D-лидара",
+ license="MIT",
+ tests_require=["pytest"],
+ entry_points={
+ "console_scripts": [
+ "detector = flyguard.node:main",
+ "player_keys = flyguard.player_keys:main",
+ ],
+ },
+)
diff --git a/ros2_ws/src/flyguard/test/test_pipeline.py b/ros2_ws/src/flyguard/test/test_pipeline.py
new file mode 100644
index 0000000..b884e36
--- /dev/null
+++ b/ros2_ws/src/flyguard/test/test_pipeline.py
@@ -0,0 +1,1271 @@
+"""Тесты конвейера, не требующие ROS.
+
+Проверяется то, что легко сломать незаметно: разбор CDR, порядок точек,
+выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
+глубины, кодирование грибовидного тела и вставка синтетического предмета.
+
+ pytest ros2_ws/src/flyguard/test
+"""
+from __future__ import annotations
+
+import sqlite3
+import struct
+import sys
+from pathlib import Path
+
+import numpy as np
+import pytest
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+ sys.path.insert(0, str(ROOT))
+
+from flyguard.cdr import point_dtype # noqa: E402
+from flyguard.geometry import RailPlane # noqa: E402
+from flyguard.lobula import cluster_by_depth # noqa: E402
+from flyguard.mushroom_body import MushroomBody, MushroomBodyConfig # noqa: E402
+from flyguard.retina import ScanLayout # noqa: E402
+
+DATA = Path(__file__).resolve().parents[4] / "data" / "for_hackathon"
+
+
+# --------------------------------------------------------------------------- CDR
+
+def test_point_dtype_handles_unaligned_timestamp():
+ """У лидара поле timestamp (float64) лежит по смещению 18 — без выравнивания."""
+ fields = [("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(fields, 26)
+ assert dt.itemsize == 26
+ assert dt["timestamp"].itemsize == 8
+
+
+def test_point_dtype_pads_trailing_gap():
+ """Хвост до point_step добивается паддингом — так устроено само сообщение."""
+ assert point_dtype([("x", 0, 7, 1)], 26).itemsize == 26
+
+
+def test_point_dtype_rejects_overlapping_fields():
+ with pytest.raises(ValueError, match="перекрыва"):
+ 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):
+ el = np.linspace(6.0, -6.0, n_rings)
+ shift = np.zeros(n_rings, np.int64) if shift is None else shift
+ return ScanLayout(el, -0.1, 2.0, shift, np.zeros(n_rings), n_az, n_echo)
+
+
+def test_rectification_undoes_channel_skew():
+ """Сдвиг строк должен ровно компенсировать азимутальный сдвиг канала."""
+ shift = np.array([-2, -1, 0, 1, 2, 0, -1, 1], np.int64)
+ lay = _layout(shift=shift)
+ j = np.arange(lay.n_az)
+ for h in range(lay.n_rings):
+ expected = np.clip(j + shift[h], 0, lay.n_az - 1)
+ assert np.array_equal(lay.gather[h], expected)
+ assert lay.gather_ok[2].all() # нулевой сдвиг — потерь нет
+
+
+def test_azimuth_grid_is_monotonic_and_centred():
+ lay = _layout(n_az=41)
+ d = np.diff(lay.az_grid_deg)
+ assert np.allclose(d, -0.1)
+ assert lay.dirs.shape == (lay.n_rings, lay.n_az, 3)
+ assert np.allclose(np.linalg.norm(lay.dirs, axis=-1), 1.0, atol=1e-5)
+
+
+def test_forward_direction_is_minus_y():
+ """Азимут 0 смотрит вперёд, а вперёд в системе сенсора — это −Y."""
+ lay = ScanLayout(np.array([0.0]), -0.1, 0.0, np.zeros(1, np.int64),
+ np.zeros(1), 1, 1)
+ assert lay.dirs[0, 0, 1] == pytest.approx(-1.0, abs=1e-6)
+
+
+# --------------------------------------------------------------------------- геометрия
+
+def test_floor_range_matches_flat_plane():
+ """Луч вниз под углом θ проходит до полотна H/sin θ — это дальность вдоль
+ луча, а не горизонтальное расстояние: именно её сравнивают с измеренной."""
+ height = 2.0
+ plane = RailPlane(a=0.0, b=0.0, c=-height, inliers=0, rms=0.0)
+ for el in (-1.0, -3.0, -10.0):
+ lay = ScanLayout(np.array([el]), -0.1, 0.0, np.zeros(1, np.int64),
+ np.zeros(1), 1, 1)
+ got = plane.floor_range(lay)[0, 0]
+ assert got == pytest.approx(height / np.sin(np.radians(-el)), rel=1e-3)
+
+
+def test_upward_rays_never_hit_floor():
+ plane = RailPlane(a=0.0, b=0.0, c=-2.0, inliers=0, rms=0.0)
+ lay = ScanLayout(np.array([5.0]), -0.1, 0.0, np.zeros(1, np.int64), np.zeros(1), 1, 1)
+ assert not np.isfinite(plane.floor_range(lay)[0, 0])
+
+
+def test_height_above_plane_accounts_for_tilt():
+ plane = RailPlane(a=0.01, b=-0.02, c=-1.5, inliers=0, rms=0.0)
+ d = np.array([10.0]); u = np.array([2.0]); z = np.array([0.0])
+ expected = 0.0 - (0.01 * 10.0 + (-0.02) * 2.0 + (-1.5))
+ assert plane.height_of(d, u, z)[0] == pytest.approx(expected)
+
+
+# --------------------------------------------------------------------------- кластеризация
+
+def test_depth_aware_clustering_splits_on_range_gap():
+ """Предмет перед далёкой стеной не должен слипнуться с ней в одно пятно."""
+ mask = np.zeros((4, 10), bool)
+ mask[:, :] = True
+ r = np.full((4, 10), 150.0, np.float32)
+ r[:, 3:6] = 55.0 # предмет на 55 м на фоне 150 м
+ labels, n = cluster_by_depth(mask, r, col_reach=1, row_reach=1)
+ assert n >= 2
+ assert len(set(labels[:, 3:6].ravel())) == 1
+ assert labels[0, 0] != labels[0, 4]
+
+
+def test_clustering_tolerance_grows_with_range():
+ """Допуск относительный: разрыв 1 м слитен на 150 м и разделим на 5 м."""
+ mask = np.ones((1, 6), bool)
+ far = np.array([[150.0, 150.0, 151.0, 151.0, 150.0, 150.0]], np.float32)
+ near = np.array([[5.0, 5.0, 6.0, 6.0, 5.0, 5.0]], np.float32)
+ _, n_far = cluster_by_depth(mask, far, col_reach=1, row_reach=0)
+ _, n_near = cluster_by_depth(mask, near, col_reach=1, row_reach=0)
+ assert n_far == 1
+ assert n_near >= 2
+
+
+def test_empty_mask_is_handled():
+ labels, n = cluster_by_depth(np.zeros((3, 3), bool), np.zeros((3, 3), np.float32))
+ assert n == 0 and labels.sum() == 0
+
+
+# --------------------------------------------------------------------------- память
+
+def test_mushroom_body_code_is_sparse_and_deterministic():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=2000, sparsity=0.01, seed=1))
+ X = np.random.default_rng(0).normal(size=(5, mb.n_pn)).astype(np.float32)
+ mb.fit_normalizer(X)
+ a, b = mb.encode(X), mb.encode(X)
+ assert a.shape == (5, mb.n_active)
+ assert mb.n_active == 20
+ assert np.array_equal(a, b)
+ assert len(set(a[0].tolist())) == mb.n_active # без повторов
+
+
+def test_learning_suppresses_only_what_was_shown():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=4000, sparsity=0.01, seed=2))
+ rng = np.random.default_rng(3)
+ familiar = rng.normal(size=(200, mb.n_pn)).astype(np.float32)
+ novel = (rng.normal(size=(50, mb.n_pn)) + 8.0).astype(np.float32)
+ mb.fit_normalizer(familiar)
+ mb.learn(familiar, rate=0.3)
+ assert mb.novelty(familiar).mean() < mb.novelty(novel).mean()
+
+
+def test_auto_rate_shrinks_with_sample_size():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=10_000, sparsity=0.001))
+ assert mb.auto_rate(1_000) > mb.auto_rate(100_000)
+ assert 0 < mb.auto_rate(10 ** 7) <= 0.5
+
+
+def test_feature_count_mismatch_is_explicit():
+ mb = MushroomBody()
+ with pytest.raises(ValueError, match="признак"):
+ mb.encode(np.zeros((1, mb.n_pn + 1), np.float32))
+
+
+# --------------------------------------------------------------------------- мост в ROS
+
+class _FakeField:
+ def __init__(self, name, offset, datatype, count=1):
+ self.name, self.offset, self.datatype, self.count = name, offset, datatype, count
+
+
+class _FakeStamp:
+ def __init__(self, sec, nanosec):
+ self.sec, self.nanosec = sec, nanosec
+
+
+class _FakeHeader:
+ def __init__(self, stamp, frame_id):
+ self.stamp, self.frame_id = stamp, frame_id
+
+
+class _FakeMsg:
+ """Достаточная имитация sensor_msgs/PointCloud2 для проверки моста."""
+
+ def __init__(self, pts, fields, point_step):
+ self.header = _FakeHeader(_FakeStamp(1788354623, 110040384), "hesai_lidar")
+ self.height, self.width = 1, pts.shape[0]
+ self.point_step = point_step
+ self.is_dense = False
+ self.fields = fields
+ self.data = pts.tobytes()
+
+
+def test_ros_bridge_matches_offline_parser():
+ """Путь через rclpy обязан давать то же, что и офлайн-разбор CDR."""
+ from flyguard import ros_conv
+
+ fields_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(fields_spec, 26)
+ rng = np.random.default_rng(11)
+ pts = np.zeros(256, dt)
+ pts["x"] = rng.normal(size=256).astype(np.float32)
+ pts["y"] = rng.normal(size=256).astype(np.float32)
+ pts["z"] = rng.normal(size=256).astype(np.float32)
+ pts["intensity"] = rng.uniform(0, 255, 256).astype(np.float32)
+ pts["ring"] = np.arange(256, dtype=np.uint16) % 128
+
+ msg = _FakeMsg(pts, [_FakeField(*f) for f in fields_spec], 26)
+ got = ros_conv.from_ros(msg)
+
+ assert got.n_points == 256
+ assert got.frame_id == "hesai_lidar"
+ assert got.stamp == pytest.approx(1788354623.110040384, abs=1e-6)
+ for name in ("x", "y", "z", "intensity", "ring"):
+ assert np.array_equal(got.points[name], pts[name])
+
+
+def test_ros_bridge_sorts_fields_by_offset():
+ """Порядок полей в сообщении не гарантирован — мост обязан его нормализовать."""
+ from flyguard import ros_conv
+
+ fields_spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1)]
+ dt = point_dtype(fields_spec, 16)
+ pts = np.zeros(8, dt)
+ pts["z"] = np.arange(8, dtype=np.float32)
+ shuffled = [_FakeField(*f) for f in reversed(fields_spec)]
+ got = ros_conv.from_ros(_FakeMsg(pts, shuffled, 16))
+ assert np.array_equal(got.points["z"], pts["z"])
+
+
+def test_ros_bridge_reports_missing_fields():
+ from flyguard import ros_conv
+
+ msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
+ ok, why = ros_conv.has_required_fields(msg)
+ assert not ok and "intensity" in why
+
+
+# --------------------------------------------------------- фигура и фон, привыкание
+
+def _cand(**kw):
+ from flyguard.lobula import Candidate
+
+ base = dict(d=60.0, u=0.1, h=1.0, d_min=59.7, h_min=0.3, width=0.4, height=1.0,
+ depth=0.5, containment=0.9, n_rays=30, n_rings=6, n_cols=5,
+ gap=8.0, on=0.001, floor_deficit=0.2, shadow=0.2, inten=40.0,
+ az_deg=0.5, el_deg=-1.0, bbox=(10, 16, 20, 25))
+ base.update(kw)
+ return Candidate(**base)
+
+
+def test_contrast_split_keeps_step_and_drops_smooth_wall():
+ """Гладкая стена не даёт фигуры, ступенька на ней — даёт."""
+ from flyguard.lobula import cluster_by_depth, split_by_figure
+
+ # стена: дальность плавно растёт вдоль строки от 20 до 120 м
+ r = np.tile(np.linspace(20.0, 120.0, 200, dtype=np.float32), (12, 1))
+ mask = np.ones(r.shape, bool)
+ labels, n = cluster_by_depth(mask, r, col_reach=3)
+ assert n == 1 # стена связна целиком
+
+ # контраст гладкой стены равен нулю — резать нечего
+ flat = np.zeros_like(r)
+ out, n_out = split_by_figure(labels, n, r, flat, max_depth=15.0, thr=6.0)
+ assert n_out == n and out is labels
+
+ # предмет: ступенька, торчащая из стены на 10 м
+ gap = np.zeros_like(r)
+ gap[4:8, 90:98] = 10.0
+ out, n_out = split_by_figure(labels, n, r, gap, max_depth=15.0, thr=6.0)
+ kept = out[out > 0]
+ assert kept.size == 32 # ровно лучи ступеньки
+ assert np.unique(kept).size == 1 # и это одна компонента
+
+
+def test_habituation_suppresses_shape_repeating_at_different_places():
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
+ seen = []
+ s = 0.0
+ for _ in range(14):
+ s += 25.0
+ c = _cand(d=60.0)
+ hab.update([c], s, 25.0)
+ seen.append(c.novelty)
+ assert seen[0] > 0.9 and seen[-1] < 0.25
+ assert hab.places >= 10
+
+
+def test_habituation_keeps_object_standing_in_one_place_novel():
+ """Подъезд к неподвижному предмету — это одно место, а не сто повторов."""
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
+ s = 0.0
+ nov = []
+ for k in range(50):
+ s += 3.0
+ nov.append(_cand(d=160.0 - 3.0 * k))
+ hab.update([nov[-1]], s, 3.0)
+ assert hab.places <= 2 # одно место (плюс дрейф оценки)
+ assert nov[-1].novelty > 0.7
+
+
+def test_habituation_forgets_after_a_long_run():
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1, recover_m=100.0))
+ s = 0.0
+ for _ in range(14):
+ s += 25.0
+ hab.update([_cand(d=60.0)], s, 25.0)
+ low = hab.level
+ hab.advance(600.0)
+ assert hab.level < low * 0.2
+
+
+# --------------------------------------------------------------- считывание MBON
+
+def test_mbon_readout_learns_and_round_trips(tmp_path):
+ """Обучается, сохраняется без потерь и даёт калиброванную вероятность."""
+ from flyguard.mbon_readout import MbonConfig, MbonReadout
+
+ rng = np.random.default_rng(3)
+ X = rng.standard_normal((2000, 23)).astype(np.float32)
+ y = ((X[:, 0] + 0.5 * X[:, 3]) > 0.3).astype(np.float32)
+ m = MbonReadout(MbonConfig(n_kc=4000, sparsity=0.02), n_pn=23)
+ m.fit_normalizer(X)
+ m.learn(X, y, epochs=60, lr=4.0)
+
+ s = m.score(X)
+ assert s.min() >= 0.0 and s.max() <= 1.0
+ assert ((s > 0.5) == (y > 0.5)).mean() > 0.8 # калибровка, не только порядок
+
+ path = tmp_path / "mbon.npz"
+ m.save(path)
+ again = MbonReadout.load(path)
+ assert np.allclose(again.score(X[:64]), s[:64], atol=1e-6)
+
+
+def test_mbon_replaces_hand_formula_and_blend_interpolates():
+ """Модель входит в вес улики, а смешивание даёт обе крайности."""
+ from flyguard.central_complex import _quality
+
+ c = _cand(gap=0.0, containment=0.3, depth=9.0, h_min=1.4, n_rays=5)
+ hand = _quality(c) # без модели
+ c.extra["mbon"] = 0.99
+ assert _quality(c, mbon_blend=1.0) > hand * 3 # модель вытягивает
+ assert _quality(c, mbon_blend=0.0) == hand # ручная формула нетронута
+ mid = _quality(c, mbon_blend=0.5)
+ assert hand < mid < _quality(c, mbon_blend=1.0)
+
+ # уверенное «это обстановка» гасит даже хорошую геометрию
+ good = _cand(gap=12.0, containment=1.0, depth=0.4, h_min=0.3, n_rays=80)
+ strong = _quality(good)
+ good.extra["mbon"] = 0.01
+ assert _quality(good, mbon_blend=1.0) < strong * 0.2
+
+
+def test_mbon_absent_leaves_pipeline_unchanged():
+ """Без модели вес улики считается ровно как раньше."""
+ from flyguard.central_complex import _quality
+
+ c = _cand()
+ assert "mbon" not in c.extra
+ assert _quality(c, mbon_blend=1.0) == _quality(c, mbon_blend=0.0)
+
+
+def test_far_warning_threshold_only_relaxes_beyond_its_distance():
+ """Послабление далёким трекам не должно трогать ближнюю зону."""
+ from flyguard.descending import DescendingNeurons
+
+ flat = DescendingNeurons()
+ assert flat._warn_at(10.0) == flat._warn_at(199.0) == flat.warn_evidence
+
+ dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.3,
+ warn_far_from=90.0, max_range=200.0)
+ assert dn._warn_at(50.0) == 0.5 # ближняя зона не тронута
+ assert dn._warn_at(90.0) == 0.5 # ровно на границе — тоже
+ assert abs(dn._warn_at(145.0) - 0.4) < 1e-6 # середина
+ assert abs(dn._warn_at(200.0) - 0.3) < 1e-6 # дальний край
+ assert abs(dn._warn_at(400.0) - 0.3) < 1e-6 # и не ниже него
+
+
+def test_novelty_fades_with_range_and_is_intact_up_close():
+ """Знакомость должна работать вблизи и не мешать вдали."""
+ from flyguard.central_complex import _quality
+
+ near, far = _cand(), _cand()
+ near.d, far.d = 40.0, 160.0
+ near.novelty = far.novelty = 0.15 # «знакомо», как далёкий предмет
+
+ # вблизи гашения нет: знакомое остаётся придавленным
+ a = _quality(near, nov_fade_from=0.0)
+ b = _quality(near, nov_fade_from=90.0)
+ assert abs(a - b) < 1e-9
+
+ # вдали вклад знакомости снят целиком
+ c = _quality(far, nov_fade_from=0.0)
+ d = _quality(far, nov_fade_from=90.0, nov_fade_to=160.0)
+ assert d > c
+
+ # и в промежутке — частично
+ mid = _cand(); mid.d, mid.novelty = 125.0, 0.15
+ m = _quality(mid, nov_fade_from=90.0, nov_fade_to=160.0)
+ assert _quality(mid, nov_fade_from=0.0) < m < d
+
+
+# ------------------------------------------------------- модель сенсора: яркость
+
+def test_injected_intensity_copies_the_surroundings():
+ """Яркость вставки берётся из записи, а не назначается.
+
+ Абсолютной шкалы интенсивности в данных нет: медиана кандидатов по бэгам
+ 3…7, а в записи с настоящим предметом 23.5. Любое назначенное число делает
+ вставку опознаваемой по одной яркости — сначала как «тускло = предмет»
+ (ламбертова ρ·cosθ/r²), потом как «ярко = предмет» (постоянные 35).
+ Поэтому у функции нет аргументов ни дальности, ни отражения.
+ """
+ import inspect
+
+ from flyguard.synth import local_intensity
+
+ # отражения среди аргументов нет; дальность есть, но только как выбор
+ # полосы реальных возвратов для лучей, ушедших в пустоту
+ args = set(inspect.signature(local_intensity).parameters)
+ assert not args & {"reflectivity", "rho", "cos_inc"}
+
+ rng = np.random.default_rng(0)
+ prev = np.array([10.0, 20.0, 0.0, 0.0], np.float32) # два луча в пустоту
+ pool = np.full(50, 30.0, np.float32)
+ frame = np.full(10_000, 7.0, np.float32)
+ v = local_intensity(prev, pool, frame, rng)
+
+ assert 8.0 < v[0] < 13.0 and 16.0 < v[1] < 25.0 # взято с тех же лучей
+ assert 24.0 < v[2] < 38.0 and 24.0 < v[3] < 38.0 # взято из запаса
+
+ # запас пуст — остаётся кадр целиком
+ v2 = local_intensity(np.zeros(200, np.float32), np.zeros(0, np.float32),
+ frame, rng)
+ assert 5.5 < float(np.median(v2)) < 9.0
+
+ # без разложения окружения по дальности сама дальность ничего не меняет
+ a = local_intensity(prev, pool, frame, np.random.default_rng(3), d=20.0)
+ b = local_intensity(prev, pool, frame, np.random.default_rng(3), d=200.0)
+ assert np.allclose(a, b)
+
+
+# --------------------------------------------------------------------------- данные
+
+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(" uint32 (_align(4))
+ buf += struct.pack(" uint32 (_align(4))
+ buf += struct.pack(" 0 and r.max() < 250
+ assert np.all(img.r_far[img.valid] >= img.r_near[img.valid] - 1e-3)
+
+
+# ------------------------------------------------------------ раскладка по бэгам
+
+_TOOLS = ROOT / "tools" # выгрузка: tools лежит рядом с тестами
+if not _TOOLS.exists():
+ _TOOLS = ROOT.parents[2] / "tools" # основной проект: ros2_ws/src/flyguard
+if str(_TOOLS) not in sys.path:
+ sys.path.insert(0, str(_TOOLS))
+
+import _parallel as _P # noqa: E402
+
+
+def _twice(x):
+ """Задача для проверки. Верхнего уровня: иначе её не передать в процесс."""
+ return x * 2
+
+
+def test_parallel_keeps_task_order_when_results_arrive_out_of_order():
+ """Считается по готовности, складывается по номеру задачи.
+
+ Ломается это незаметно и опасно: цифры остаются правдоподобными, просто
+ приписываются не тому бэгу. Поэтому проверяется не «столько же строк», а
+ что результат каждой задачи лёг на своё место.
+ """
+ tasks = list(range(7))
+ want = [(t, t * 2) for t in tasks]
+
+ seq = [None] * len(tasks)
+ for i, t, r, _ in _P.run(_twice, tasks, jobs=1):
+ seq[i] = (t, r)
+
+ par = [None] * len(tasks)
+ for i, t, r, _ in _P.run(_twice, tasks, jobs=4):
+ par[i] = (t, r)
+
+ assert seq == want
+ assert par == want
+
+
+def test_parallel_never_starts_more_processes_than_there_are_bags():
+ """Бэгов пять, и шестой процесс занять нечем."""
+ assert _P.resolve(0, 1) == 1 # одна задача — без пула вовсе
+ assert _P.resolve(8, 3) == 3 # просили больше, чем есть работы
+ assert _P.resolve(1, 5) == 1 # явная последовательная отладка
+ assert 1 <= _P.resolve(0, 5) <= 5
+
+
+# ------------------------------------------------------------ решение по треку
+
+def _pair_of_tracks():
+ """Два трека с ОДИНАКОВОЙ насыщенной уликой и разным средним весом."""
+ from flyguard.central_complex import CentralComplex, Track
+
+ cx = CentralComplex()
+ cx.s_world = 0.0
+ good = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
+ evidence=1.0, hits=9, age=9, novelty=0.6,
+ n_obs=9, w_sum=9 * 0.95, p_sum=9 * 0.95)
+ junk = Track(id=2, s_world=70.0, u=0.5, h=0.3, width=0.5, height=1.7,
+ evidence=1.0, hits=9, age=9, novelty=0.6,
+ n_obs=9, w_sum=9 * 0.27, p_sum=9 * 0.27)
+ cx.tracks = [good, junk]
+ return cx
+
+
+def test_saturated_evidence_stops_separating_and_the_mean_weight_does_not():
+ """Улика обрезана единицей — и перестаёт различать.
+
+ На реальном объекте медиана улики 1.000 и у предмета, и у ложных треков
+ (AUC 0.624), потому что сумма упёрлась в потолок у обоих. Средний вес
+ наблюдения при этом 0.998 против 0.269. Здесь это проверяется на паре
+ треков, у которых улика одинакова НАРОЧНО.
+ """
+ from flyguard.descending import DescendingNeurons
+
+ plain = DescendingNeurons().decide(_pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in plain.objects} == {1, 2}
+
+ picked = DescendingNeurons(track_blend=0.7, track_score="w_mean").decide(
+ _pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in picked.objects} == {1}
+
+
+def test_track_score_without_a_model_falls_back_to_evidence():
+ """`model` без файла модели обязан вести себя как раньше, а не как ноль.
+
+ Отсчёт модели по умолчанию нулевой, и молчаливое его использование
+ погасило бы вообще все треки.
+ """
+ from flyguard.descending import DescendingNeurons
+
+ dn = DescendingNeurons(track_blend=0.7, track_score="model",
+ track_readout=None)
+ out = dn.decide(_pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in out.objects} == {1, 2}
+
+
+def test_track_accumulators_do_not_touch_the_evidence():
+ """Сводка по треку копится отдельно и в улику не вмешивается."""
+ from flyguard.central_complex import CentralComplex
+ from flyguard.lobula import Candidate
+
+ cx = CentralComplex()
+ c = Candidate(d=60.0, u=0.0, h=0.5, d_min=59.8, h_min=0.1, width=0.4,
+ height=1.6, depth=0.3, containment=0.9, n_rays=40,
+ n_rings=8, n_cols=6, gap=4.0, on=0.2, floor_deficit=0.0,
+ shadow=0.3, inten=12.0, az_deg=0.0, el_deg=-1.0,
+ bbox=(0, 0, 8, 6), novelty=0.5)
+ cx.update([c], ds=0.0, dt=0.1)
+ t = cx.tracks[0]
+ assert t.n_obs == 1 and t.w_mean > 0.0
+ assert abs(t.evidence - cx.gain * t.w_mean) < 1e-6
+
+
+def test_track_p_mean_falls_back_when_there_is_no_readout():
+ """Без модели покадрового считывания отсчёта нет, и ноль тут погасил бы всё."""
+ from flyguard.central_complex import Track
+
+ t = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
+ n_obs=4, w_sum=4 * 0.8)
+ assert t.p_n == 0
+ assert abs(t.p_mean - 0.8) < 1e-9
+ t.p_n, t.p_sum = 4, 4 * 0.3
+ assert abs(t.p_mean - 0.3) < 1e-9
+
+
+def test_track_gate_removes_the_weak_track_without_touching_the_strong_one():
+ """Порог ничего не отнимает у прошедшего — в отличие от смешивания."""
+ from flyguard.descending import DescendingNeurons
+
+ out = DescendingNeurons(track_gate=0.5, track_score="w_mean").decide(
+ _pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in out.objects} == {1}
+ assert out.objects[0].confidence == pytest.approx(
+ DescendingNeurons().decide(_pair_of_tracks(), speed=10.0).objects[0].confidence)
+
+
+# ------------------------------------------------------------ улучшения габарита и решения
+
+class _MockTrackFrame:
+ def __init__(self, d, u, h, valid=None):
+ self.d = np.asarray(d, np.float32)
+ self.u = np.asarray(u, np.float32)
+ self.h = np.asarray(h, np.float32)
+ self.valid = np.ones_like(self.d, dtype=bool) if valid is None else np.asarray(valid, bool)
+
+ def lateral(self, corridor=None):
+ if corridor is None or corridor.n_slices == 0:
+ return self.u
+ curved = self.u - corridor.centre(self.d)
+ return np.where(np.abs(curved) < np.abs(self.u), curved, self.u)
+
+
+def test_gauge_mask_zonal_floor_and_corridor_expansion():
+ """Проверка зонального пола и расширения габарита по неопределённости оси."""
+ from flyguard.geometry import STRAIGHT, Corridor
+ from flyguard.lobula import gauge_mask
+
+ # 1. Зональный пол: в колее (|u| <= 0.85) порог 0.16 м, снаружи — 0.28 м
+ tf = _MockTrackFrame(
+ d=[50.0, 50.0],
+ u=[0.4, 1.2],
+ h=[0.20, 0.20] # оба 20 см над рельсом
+ )
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, h_lo_core=0.16, core_width=0.85)
+ assert inside[0] # в колее 0.20 м > 0.16 м — внутри габарита!
+ assert not inside[1] # на периферии 0.20 м < 0.28 м — отсечено!
+
+ # 2. Расширение габарита на дальней дистанции при кривизне
+ corridor = Corridor(np.array([0.0, 0.0, 0.0003], np.float32), 60.0, 10, 1600.0)
+ tf_far = _MockTrackFrame(
+ d=[150.0],
+ u=[1.90],
+ h=[1.0]
+ )
+ # Без расширения (k_sigma=0): |u| = 1.9 > 1.6 -> отсечено
+ inside_rigid, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, k_sigma=0.0)
+ assert not inside_rigid[0]
+
+ # С расширением (k_sigma=0.75): w_eff > 1.9 м -> объект удержан
+ inside_exp, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, k_sigma=0.75)
+ assert inside_exp[0]
+
+
+def test_core_floor_is_not_lowered_closer_than_core_from():
+ """Вблизи опущенный пол открывает головки рельсов — там он не действует."""
+ from flyguard.geometry import STRAIGHT
+ from flyguard.lobula import gauge_mask
+
+ tf = _MockTrackFrame(d=[20.0, 60.0, 60.0], u=[0.4, 0.4, 1.2], h=[0.20, 0.20, 0.20])
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, h_lo_core=0.16,
+ core_width=0.85, core_from=55.0)
+ assert not inside[0] # 20 м — ближе порога, пол прежний
+ assert inside[1] # 60 м в колее — пол опущен
+ assert not inside[2] # вне колеи пол не опускается нигде
+
+
+def test_gauge_mask_filters_platform_surface():
+ """Настил платформы (h~1.1-1.2 м, |u| >= 1.30 м) отсекается, а препятствие в габарите остаётся."""
+ from flyguard.geometry import STRAIGHT
+ from flyguard.lobula import gauge_mask
+
+ tf = _MockTrackFrame(
+ d=[40.0, 40.0],
+ u=[1.40, 0.90],
+ h=[1.15, 1.15]
+ )
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, platform_filter=True)
+ assert not inside[0] # настил платформы отсечён
+ assert inside[1] # препятствие на колее сохранено
+
+
+def test_descending_far_soft_warning_triggers_without_emergency():
+ """На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
+ from flyguard.central_complex import CentralComplex, Track
+ from flyguard.descending import DescendingNeurons
+
+ cx = CentralComplex()
+ cx.s_world = 0.0
+ far_track = Track(
+ id=1, s_world=130.0, u=0.2, h=0.5, width=0.5, height=1.6,
+ evidence=0.20, hits=3, age=5, novelty=0.12,
+ n_obs=3, w_sum=3 * 0.85, p_sum=3 * 0.85
+ )
+ cx.tracks = [far_track]
+
+ dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.35, warn_far_from=90.0,
+ far_channel=True)
+ decision = dn.decide(cx, speed=15.0)
+
+ assert decision.detected is True
+ assert decision.emergency is False
+ assert decision.distance == pytest.approx(130.0, abs=1.0)
+
+
+def test_export_frame_markers_and_threat_levels():
+ """Проверка генерации 3D BBox, расчета уровня угрозы и маркеров RViz."""
+ from flyguard.descending import Decision, DetectedObject
+ from flyguard.export import ThreatLevel, export_frame
+ from flyguard.geometry import RailPlane, STRAIGHT
+
+ plane = RailPlane(a=0.01, b=0.0, c=-1.80, inliers=100, rms=0.02)
+ dec = Decision(
+ detected=True,
+ emergency=False,
+ distance=45.0,
+ ttc=4.5,
+ speed=10.0,
+ stopping_distance=50.0,
+ objects=[
+ DetectedObject(
+ distance=45.0, lateral=0.3, height=0.6, width=0.5, size_v=1.2,
+ confidence=0.92, novelty=0.8, n_rays=15, track_id=42, ttc=4.5
+ )
+ ]
+ )
+
+ exp = export_frame(dec, plane, STRAIGHT, stamp=1710000000.5)
+ assert exp.threat_level == ThreatLevel.WARNING
+ assert len(exp.boxes) == 1
+ b = exp.boxes[0]
+ assert b.track_id == 42
+ assert b.distance_along_track == 45.0
+ assert b.x == pytest.approx(0.3)
+ assert b.y == pytest.approx(-45.0)
+ # z = h + a*d + b*u + c = 0.6 + 0.01*45 + 0.0 + (-1.8) = -0.75
+ assert b.z == pytest.approx(0.6 + 0.01 * 45.0 - 1.80)
+
+ # Проверка сериализации в словарь и RViz маркеры
+ d = exp.to_dict()
+ assert d["threat_level"] == "WARNING"
+ assert d["n_objects"] == 1
+
+ markers = exp.to_rviz_markers(frame_id="hesai_pandar")
+ assert len(markers) >= 2 # bbox cube + text label
+
+
+def test_benchmark_augmentation_logic():
+ """Проверка генерации сценариев и логики аугментаций в make_benchmark."""
+ from make_benchmark import _ScenarioState
+ from flyguard.pipeline import FlyGuard, Params
+
+ rng = np.random.default_rng(42)
+ p = FlyGuard(Params())
+
+ # 1. Спавн в диапазоне [40, 200] м
+ d_start_min, d_start_max = 40.0, 200.0
+ spawns = [float(rng.uniform(d_start_min, d_start_max)) for _ in range(100)]
+ assert all(40.0 <= d <= 200.0 for d in spawns)
+ assert min(spawns) < 60.0 and max(spawns) > 180.0
+
+ # 2. Боковой дрейф для пешеходов
+ v_lat_max = 0.35
+ v_lats = [float(rng.uniform(-v_lat_max, v_lat_max)) for _ in range(100)]
+ assert all(-0.35 <= v <= 0.35 for v in v_lats)
+
+ # 3. Шум по дальности
+ s_noise_std = 0.12
+ noises = [float(np.clip(rng.normal(0.0, s_noise_std), -0.35, 0.35)) for _ in range(500)]
+ assert abs(np.mean(noises)) < 0.05
+ assert 0.08 < np.std(noises) < 0.15
+
+ # 4. _ScenarioState
+ sc = _ScenarioState(name="человек_лежа", lat_base=0.0, pipe=p,
+ d_init=120.0, v_lat=0.15, is_static=False)
+ assert sc.name == "человек_лежа"
+ assert sc.v_lat == 0.15
+ assert not sc.is_static
+
+
+def test_device_detection_and_fallback():
+ """Проверка автоматического определения GPU устройства и fallback на CPU."""
+ from flyguard.device import get_device, get_device_info, is_cuda_available, to_numpy
+
+ dev = get_device("auto")
+ assert dev in ("cpu", "cuda") or dev.startswith("cuda:")
+
+ if not is_cuda_available():
+ assert get_device("cuda") == "cpu"
+ assert get_device("cpu") == "cpu"
+
+ info = get_device_info()
+ assert "device" in info
+ assert "torch_available" in info
+
+ arr = np.array([1.0, 2.0, 3.0], dtype=np.float32)
+ assert np.array_equal(to_numpy(arr), arr)
+
+
+def test_lamina_device_routing():
+ """Проверка работы Lamina с указанием устройства (cpu / auto)."""
+ from flyguard import lamina
+
+ r = np.full((16, 32), 20.0, dtype=np.float32)
+ r[8, 16] = 5.0 # препятствие
+ valid = np.ones((16, 32), dtype=bool)
+
+ out_cpu = lamina.process(r, valid, device="cpu")
+ out_auto = lamina.process(r, valid, device="auto")
+
+ assert out_cpu.on.shape == (16, 32)
+ assert out_auto.on.shape == (16, 32)
+ assert out_cpu.on[8, 16] > 0.0
+ assert np.allclose(out_cpu.on, out_auto.on, atol=1e-5)
+
+
+def test_device_runtime_failure_and_fallback():
+ """Проверка динамического перехода на CPU при сбое/отвале GPU в рантайме."""
+ from flyguard.device import is_cuda_available, get_device, notify_cuda_error, reset_device_cache
+ from flyguard import lamina
+
+ # Симуляция критического сбоя GPU
+ notify_cuda_error(RuntimeError("Simulated CUDA device disconnect / OOM"))
+ try:
+ assert not is_cuda_available()
+ assert get_device("cuda") == "cpu"
+ assert get_device("auto") == "cpu"
+
+ r = np.full((16, 32), 20.0, dtype=np.float32)
+ r[8, 16] = 4.0
+ valid = np.ones((16, 32), dtype=bool)
+
+ # Даже при явном указании device="cuda", Lamina должна успешно отработать на CPU
+ out = lamina.process(r, valid, device="cuda")
+ assert out.on.shape == (16, 32)
+ assert out.on[8, 16] > 0.0
+ finally:
+ reset_device_cache()
+
+
+
+def test_scenario_stream_does_not_depend_on_the_rest_of_the_catalogue():
+ """Новый предмет в каталоге не сдвигает случайность остальных сценариев.
+
+ Раньше генератор был один на запись, и сценарии тянули из него по очереди:
+ добавили человека лёжа — поменялись цифры по ящику. Теперь поток сценария
+ определяется только зерном, именем предмета и поперечным смещением.
+ """
+ from make_benchmark import scenario_rng
+
+ a = scenario_rng(12345, "ящик", 0.9).random(5)
+ assert np.array_equal(a, scenario_rng(12345, "ящик", 0.9).random(5))
+ assert not np.array_equal(a, scenario_rng(12345, "ящик", 0.0).random(5))
+ assert not np.array_equal(a, scenario_rng(12345, "чемодан", 0.9).random(5))
+ assert not np.array_equal(a, scenario_rng(777, "ящик", 0.9).random(5))
+
+
+def test_export_box_follows_a_curved_track():
+ """В кривой рамка стоит на пути и повёрнута вдоль него.
+
+ Боковое смещение трека отсчитано от оси пути, и без поправки на её
+ положение рамка на 150 м при радиусе 1300 м стояла в 8.6 м от пути, а
+ поворот был зеркальным — 13° мимо направления пути.
+ """
+ import math
+ from flyguard.descending import Decision, DetectedObject
+ from flyguard.export import export_frame
+ from flyguard.geometry import Corridor, RailPlane
+
+ R = 1300.0
+ cor = Corridor(np.array([0.0, 0.0, 1.0 / (2 * R)], np.float32), 200.0, 10, 1600.0)
+ plane = RailPlane(a=0.0, b=0.0, c=-1.8, inliers=100, rms=0.02)
+ for d in (55.0, 100.0, 150.0):
+ dec = Decision(detected=True, distance=d, speed=10.0, stopping_distance=30.0,
+ objects=[DetectedObject(distance=d, lateral=0.3, height=0.8,
+ width=0.5, size_v=1.7, confidence=0.9,
+ novelty=0.6, n_rays=10, track_id=1,
+ ttc=d / 10)])
+ box = export_frame(dec, plane, cor).boxes[0]
+ on_track = float(cor.centre(np.array([d], np.float32))[0]) + 0.3
+ assert box.x == pytest.approx(on_track, abs=1e-3)
+
+ slope = d / R
+ long_axis = np.array([-math.sin(box.yaw), math.cos(box.yaw)])
+ tangent = np.array([slope, -1.0]) / math.hypot(slope, 1.0)
+ assert abs(float(long_axis @ tangent)) == pytest.approx(1.0, abs=1e-6)
+
+
+def test_track_carries_the_sensor_lateral_for_drawing():
+ """Рамка в RViz стоит у своих точек, а не у оси пути.
+
+ На кривой синтетики организаторов (радиус около 760 м) предмет на оси
+ пути на 39 м лежит в 1.05 м левее оси лидара, а рамка рисовалась по
+ смещению от оси пути — на +0.13 м, в 1.2 м от своих точек.
+ """
+ from flyguard.central_complex import CentralComplex
+ from flyguard.descending import DescendingNeurons
+ from flyguard.lobula import Candidate
+
+ cx = CentralComplex()
+ c = Candidate(d=39.0, u=0.02, h=0.3, d_min=38.8, h_min=0.1, width=0.3,
+ height=0.3, depth=0.2, containment=1.0, n_rays=20,
+ n_rings=5, n_cols=4, gap=6.0, on=0.2, floor_deficit=0.0,
+ shadow=0.3, inten=12.0, az_deg=-1.5, el_deg=-1.0,
+ bbox=(0, 0, 5, 4), novelty=0.6, extra={"u_raw": -1.05})
+ cx.update([c], ds=0.0, dt=0.1)
+ t = cx.tracks[0]
+ assert t.u == pytest.approx(0.02) and t.u_raw == pytest.approx(-1.05)
+
+ t.evidence, t.hits, t.age = 1.0, 9, 9
+ obj = DescendingNeurons().decide(cx, speed=10.0).objects[0]
+ assert obj.lateral == pytest.approx(0.02) # смещение от оси пути — в сообщении
+ assert obj.sensor_x == pytest.approx(-1.05) # а рисуется рамка здесь
+
+
+def test_gauge_outline_bends_with_the_track():
+ """Контур габарита в RViz идёт вдоль оценённой оси, а не прямо."""
+ from flyguard.export import gauge_outline
+ from flyguard.geometry import Corridor
+
+ boxes = [(1.2, 0.28, 2.3, 4.0, 200.0)]
+ straight = np.array(gauge_outline(boxes))
+ assert set(np.round(np.abs(straight[:, 0]), 6)) == {1.2}
+
+ R = 760.0
+ cor = Corridor(np.array([0.0, 0.0, -1.0 / (2 * R)], np.float32), 200.0, 10, R)
+ bent = np.array(gauge_outline(boxes, cor.centre))
+ at100 = bent[np.isclose(bent[:, 1], -100.0)]
+ c100 = float(cor.centre(np.array([100.0], np.float32))[0])
+ assert c100 == pytest.approx(-100.0 ** 2 / (2 * R), rel=1e-3)
+ assert at100[:, 0].min() == pytest.approx(c100 - 1.2, abs=1e-4)
+ assert at100[:, 0].max() == pytest.approx(c100 + 1.2, abs=1e-4)
+
+
+# ------------------------------------------------------------ видеокарта
+
+def _cuda_or_skip():
+ torch = pytest.importorskip("torch")
+ if not torch.cuda.is_available():
+ pytest.skip("нет видеокарты с CUDA")
+
+
+def _scene(seed=0, shape=(128, 600)):
+ """Образ, похожий на тоннель: гладкие стены, ступеньки-предметы, пропуски эха."""
+ rng = np.random.default_rng(seed)
+ h, w = shape
+ r = (np.linspace(3.0, 180.0, w)[None, :] * (1.0 + 0.2 * np.sin(np.arange(h)[:, None] / 9.0))
+ ).astype(np.float32)
+ for _ in range(40):
+ i, j = rng.integers(0, h - 8), rng.integers(0, w - 12)
+ r[i:i + rng.integers(2, 8), j:j + rng.integers(2, 12)] *= np.float32(rng.uniform(0.3, 0.9))
+ r += rng.normal(0.0, 0.02, r.shape).astype(np.float32)
+ valid = rng.random(shape) > 0.1
+ return np.where(valid, r, np.float32(0.0)), valid
+
+
+def test_gpu_lamina_and_clustering_match_cpu_bit_for_bit():
+ """Ламина и кластеризация на видеокарте — те же числа и те же номера компонент."""
+ _cuda_or_skip()
+ import torch
+ from flyguard import lamina
+ from flyguard.gpu import GpuStages
+ from flyguard.lobula import cluster_by_depth
+
+ g = GpuStages("cuda")
+ for seed in range(3):
+ r, valid = _scene(seed)
+ g.r = torch.as_tensor(r, device="cuda")
+ g.valid = torch.as_tensor(valid, device="cuda")
+ a, b = lamina._process_cpu(r, valid), g.lamina()
+ for f in ("disp", "on", "off", "on_scale", "surround", "hole"):
+ assert np.array_equal(getattr(a, f), getattr(b, f)), f
+ mask = valid & (np.random.default_rng(seed).random(r.shape) < 0.5)
+ la, na = cluster_by_depth(mask, r)
+ lb, nb = g.cluster_by_depth(mask, r)
+ assert na == nb and np.array_equal(la, lb)
+
+
+def test_gpu_projection_matches_cpu():
+ """Сетчатка на видеокарте: целый кадр — бит в бит, перемешанный — почти."""
+ _cuda_or_skip()
+ from flyguard.bag import Bag
+ from flyguard.cdr import PointCloud2
+ from flyguard.gpu import GpuStages
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
+ clouds = [pc for _, pc in bag.frames(start=2, stop=16)]
+ lay = ScanLayout.calibrate(clouds[:12])
+ cols = lay.column_slice(30.0)
+ g = GpuStages("cuda")
+ pc = clouds[-1]
+ a, b = lay.project(pc, cols), g.project(lay, pc, cols)
+ for f in ("r_near", "r_far", "inten", "valid"):
+ assert np.array_equal(getattr(a, f), getattr(b, f)), f
+
+ # порядок точек сбит, как в синтетике организаторов: раскладка по углам
+ rng = np.random.default_rng(0)
+ mixed = PointCloud2(pc.stamp, pc.frame_id, 1, pc.n_points, pc.point_step,
+ pc.is_dense, pc.points[rng.permutation(pc.n_points)])
+ a, b = lay.project(mixed, cols), g.project(lay, mixed, cols)
+ assert a.valid.sum() > 1000
+ differ = sum(int((getattr(a, f) != getattr(b, f)).sum())
+ for f in ("r_near", "r_far", "inten", "valid"))
+ assert differ < 1e-3 * a.valid.size # последний знак арктангенса и порядок равных
+
+
+def test_pipeline_survives_gpu_failure_mid_frame():
+ """Отказ видеокарты посреди кадра: кадр досчитан на процессоре, решение то же."""
+ from flyguard.bag import Bag
+ from flyguard.device import reset_device_cache
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+
+ class Broken:
+ def project(self, *a, **k):
+ raise RuntimeError("CUDA error: device lost (имитация)")
+
+ lamina = cluster_by_depth = project
+
+ frames = [pc for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=30)]
+ ref = FlyGuard(Params(device="cpu"))
+ fg = FlyGuard(Params(device="cpu"))
+ try:
+ for i, pc in enumerate(frames):
+ if i == 20: # видеокарта «была» и отказала
+ fg.gpu, fg.device = Broken(), "cuda"
+ a, b = ref.process(pc), fg.process(pc)
+ assert (a is None) == (b is None)
+ if a is not None:
+ assert a.decision.detected == b.decision.detected
+ assert [c.d for c in a.candidates] == [c.d for c in b.candidates]
+ assert fg.gpu is None and fg.device == "cpu"
+ assert "device lost" in fg.gpu_error
+ finally:
+ reset_device_cache()
+
+
+class _CpuStages:
+ """Подмена видеокарты: те же стадии на процессоре, прогрев ждёт сигнала."""
+
+ gate = None
+
+ def __init__(self, device):
+ self.calls = 0
+ self._img = None
+
+ def warmup(self):
+ if self.gate is not None:
+ self.gate.wait()
+
+ def project(self, L, pc, cols):
+ self.calls += 1
+ self._img = L.project(pc, cols)
+ return self._img
+
+ def lamina(self):
+ from flyguard import lamina
+ return lamina.process(self._img.r_near, self._img.valid, device="cpu")
+
+ def cluster_by_depth(self, mask, r, **kw):
+ return cluster_by_depth(mask, r, **kw)
+
+
+def _wait_gpu(fg, timeout=5.0):
+ import time
+ t_end = time.monotonic() + timeout
+ while fg.gpu_pending and time.monotonic() < t_end:
+ time.sleep(0.01)
+ assert not fg.gpu_pending, "поток видеокарты не закончил"
+
+
+def test_gpu_comes_up_in_background_while_cpu_counts(monkeypatch):
+ """Прогрев видеокарты идёт в фоне: кадры тем временем считает процессор,
+ а на видеокарту конвейер переходит только с начала кадра — с тем же итогом.
+ Зависший прогрев (здесь — до 20-го кадра) узел не держит."""
+ import threading
+ import flyguard.device as device
+ import flyguard.gpu as gpu
+ from flyguard.bag import Bag
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ made = []
+
+ class Stages(_CpuStages):
+ gate = threading.Event()
+
+ def __init__(self, dev):
+ super().__init__(dev)
+ made.append(self)
+
+ monkeypatch.setattr(device, "get_device", lambda preferred="auto": "cuda")
+ monkeypatch.setattr(gpu, "GpuStages", Stages)
+ frames = [pc for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=30)]
+ ref = FlyGuard(Params(device="cpu"))
+ fg = FlyGuard(Params(device="auto"), gpu_background=True)
+ try:
+ for i, pc in enumerate(frames):
+ if i == 20:
+ assert fg.gpu_pending and fg.gpu is None and fg.device == "cpu"
+ Stages.gate.set()
+ _wait_gpu(fg)
+ assert fg.gpu_active and fg.gpu is None # готова, но ждёт начала кадра
+ a, b = ref.process(pc), fg.process(pc)
+ assert (a is None) == (b is None)
+ if a is not None:
+ assert a.decision.detected == b.decision.detected
+ assert [c.d for c in a.candidates] == [c.d for c in b.candidates]
+ assert fg.gpu is made[0] and fg.device == "cuda"
+ assert made[0].calls == len(frames) - 20
+ finally:
+ Stages.gate.set()
+
+
+def test_gpu_failing_to_warm_up_leaves_cpu(monkeypatch):
+ """Прогрев в фоне упал: конвейер остаётся на процессоре, причина — в gpu_error."""
+ import flyguard.device as device
+ import flyguard.gpu as gpu
+ from flyguard.device import reset_device_cache
+ from flyguard.pipeline import FlyGuard, Params
+
+ class Bad(_CpuStages):
+ def warmup(self):
+ raise RuntimeError("CUDA error: no kernel image is available (имитация)")
+
+ monkeypatch.setattr(device, "get_device", lambda preferred="auto": "cuda")
+ monkeypatch.setattr(gpu, "GpuStages", Bad)
+ try:
+ fg = FlyGuard(Params(device="auto"), gpu_background=True)
+ _wait_gpu(fg)
+ assert fg.gpu is None and not fg.gpu_active and fg.device == "cpu"
+ assert "no kernel image" in fg.gpu_error
+ finally:
+ reset_device_cache()
+
+
+def test_brain_view_scales_for_high_resolution():
+ """Мозг для экрана в 2×: вдвое больше по обеим осям, облако из атласа
+ своего размера, а в целом та же картинка, что у узла."""
+ cv2 = pytest.importorskip("cv2")
+ # в выгрузке для команды вида мозга нет — там тест пропускается
+ BrainHybrid = pytest.importorskip("flyguard.brain_hybrid").BrainHybrid
+ from flyguard.bag import Bag
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ one, two = BrainHybrid(), BrainHybrid(scale=2)
+ if not one.enabled:
+ pytest.skip("нет атласа нейронов")
+ fg = FlyGuard(Params(enable_looming=True))
+ a = b = None
+ for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=20):
+ res = fg.process(pc, keep_debug=True)
+ if res is not None:
+ a, b = one.render(res), two.render(res)
+ assert a is not None
+ assert b.shape == (2 * a.shape[0], 2 * a.shape[1], 3)
+ small = cv2.resize(b, (a.shape[1], a.shape[0]), interpolation=cv2.INTER_AREA)
+ assert abs(float(small.mean()) - float(a.mean())) < 0.15 * float(a.mean())
diff --git a/ros2_ws/src/flyguard/test/test_pipeline.py.orig b/ros2_ws/src/flyguard/test/test_pipeline.py.orig
new file mode 100644
index 0000000..b64d665
--- /dev/null
+++ b/ros2_ws/src/flyguard/test/test_pipeline.py.orig
@@ -0,0 +1,1180 @@
+"""Тесты конвейера, не требующие ROS.
+
+Проверяется то, что легко сломать незаметно: разбор CDR, порядок точек,
+выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
+глубины, кодирование грибовидного тела и вставка синтетического предмета.
+
+ pytest ros2_ws/src/flyguard/test
+"""
+from __future__ import annotations
+
+import sys
+from pathlib import Path
+
+import numpy as np
+import pytest
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+ sys.path.insert(0, str(ROOT))
+
+from flyguard.cdr import point_dtype # noqa: E402
+from flyguard.geometry import RailPlane # noqa: E402
+from flyguard.lobula import cluster_by_depth # noqa: E402
+from flyguard.mushroom_body import MushroomBody, MushroomBodyConfig # noqa: E402
+from flyguard.retina import ScanLayout # noqa: E402
+
+DATA = Path(__file__).resolve().parents[4] / "data" / "for_hackathon"
+
+
+# --------------------------------------------------------------------------- CDR
+
+def test_point_dtype_handles_unaligned_timestamp():
+ """У лидара поле timestamp (float64) лежит по смещению 18 — без выравнивания."""
+ fields = [("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(fields, 26)
+ assert dt.itemsize == 26
+ assert dt["timestamp"].itemsize == 8
+
+
+def test_point_dtype_pads_trailing_gap():
+ """Хвост до point_step добивается паддингом — так устроено само сообщение."""
+ assert point_dtype([("x", 0, 7, 1)], 26).itemsize == 26
+
+
+def test_point_dtype_rejects_overlapping_fields():
+ with pytest.raises(ValueError, match="перекрыва"):
+ point_dtype([("x", 0, 8, 1), ("y", 4, 7, 1)], 26)
+
+
+# --------------------------------------------------------------------------- решётка
+
+def _layout(n_rings=8, n_az=40, n_echo=2, shift=None):
+ el = np.linspace(6.0, -6.0, n_rings)
+ shift = np.zeros(n_rings, np.int64) if shift is None else shift
+ return ScanLayout(el, -0.1, 2.0, shift, np.zeros(n_rings), n_az, n_echo)
+
+
+def test_rectification_undoes_channel_skew():
+ """Сдвиг строк должен ровно компенсировать азимутальный сдвиг канала."""
+ shift = np.array([-2, -1, 0, 1, 2, 0, -1, 1], np.int64)
+ lay = _layout(shift=shift)
+ j = np.arange(lay.n_az)
+ for h in range(lay.n_rings):
+ expected = np.clip(j + shift[h], 0, lay.n_az - 1)
+ assert np.array_equal(lay.gather[h], expected)
+ assert lay.gather_ok[2].all() # нулевой сдвиг — потерь нет
+
+
+def test_azimuth_grid_is_monotonic_and_centred():
+ lay = _layout(n_az=41)
+ d = np.diff(lay.az_grid_deg)
+ assert np.allclose(d, -0.1)
+ assert lay.dirs.shape == (lay.n_rings, lay.n_az, 3)
+ assert np.allclose(np.linalg.norm(lay.dirs, axis=-1), 1.0, atol=1e-5)
+
+
+def test_forward_direction_is_minus_y():
+ """Азимут 0 смотрит вперёд, а вперёд в системе сенсора — это −Y."""
+ lay = ScanLayout(np.array([0.0]), -0.1, 0.0, np.zeros(1, np.int64),
+ np.zeros(1), 1, 1)
+ assert lay.dirs[0, 0, 1] == pytest.approx(-1.0, abs=1e-6)
+
+
+# --------------------------------------------------------------------------- геометрия
+
+def test_floor_range_matches_flat_plane():
+ """Луч вниз под углом θ проходит до полотна H/sin θ — это дальность вдоль
+ луча, а не горизонтальное расстояние: именно её сравнивают с измеренной."""
+ height = 2.0
+ plane = RailPlane(a=0.0, b=0.0, c=-height, inliers=0, rms=0.0)
+ for el in (-1.0, -3.0, -10.0):
+ lay = ScanLayout(np.array([el]), -0.1, 0.0, np.zeros(1, np.int64),
+ np.zeros(1), 1, 1)
+ got = plane.floor_range(lay)[0, 0]
+ assert got == pytest.approx(height / np.sin(np.radians(-el)), rel=1e-3)
+
+
+def test_upward_rays_never_hit_floor():
+ plane = RailPlane(a=0.0, b=0.0, c=-2.0, inliers=0, rms=0.0)
+ lay = ScanLayout(np.array([5.0]), -0.1, 0.0, np.zeros(1, np.int64), np.zeros(1), 1, 1)
+ assert not np.isfinite(plane.floor_range(lay)[0, 0])
+
+
+def test_height_above_plane_accounts_for_tilt():
+ plane = RailPlane(a=0.01, b=-0.02, c=-1.5, inliers=0, rms=0.0)
+ d = np.array([10.0]); u = np.array([2.0]); z = np.array([0.0])
+ expected = 0.0 - (0.01 * 10.0 + (-0.02) * 2.0 + (-1.5))
+ assert plane.height_of(d, u, z)[0] == pytest.approx(expected)
+
+
+# --------------------------------------------------------------------------- кластеризация
+
+def test_depth_aware_clustering_splits_on_range_gap():
+ """Предмет перед далёкой стеной не должен слипнуться с ней в одно пятно."""
+ mask = np.zeros((4, 10), bool)
+ mask[:, :] = True
+ r = np.full((4, 10), 150.0, np.float32)
+ r[:, 3:6] = 55.0 # предмет на 55 м на фоне 150 м
+ labels, n = cluster_by_depth(mask, r, col_reach=1, row_reach=1)
+ assert n >= 2
+ assert len(set(labels[:, 3:6].ravel())) == 1
+ assert labels[0, 0] != labels[0, 4]
+
+
+def test_clustering_tolerance_grows_with_range():
+ """Допуск относительный: разрыв 1 м слитен на 150 м и разделим на 5 м."""
+ mask = np.ones((1, 6), bool)
+ far = np.array([[150.0, 150.0, 151.0, 151.0, 150.0, 150.0]], np.float32)
+ near = np.array([[5.0, 5.0, 6.0, 6.0, 5.0, 5.0]], np.float32)
+ _, n_far = cluster_by_depth(mask, far, col_reach=1, row_reach=0)
+ _, n_near = cluster_by_depth(mask, near, col_reach=1, row_reach=0)
+ assert n_far == 1
+ assert n_near >= 2
+
+
+def test_empty_mask_is_handled():
+ labels, n = cluster_by_depth(np.zeros((3, 3), bool), np.zeros((3, 3), np.float32))
+ assert n == 0 and labels.sum() == 0
+
+
+# --------------------------------------------------------------------------- память
+
+def test_mushroom_body_code_is_sparse_and_deterministic():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=2000, sparsity=0.01, seed=1))
+ X = np.random.default_rng(0).normal(size=(5, mb.n_pn)).astype(np.float32)
+ mb.fit_normalizer(X)
+ a, b = mb.encode(X), mb.encode(X)
+ assert a.shape == (5, mb.n_active)
+ assert mb.n_active == 20
+ assert np.array_equal(a, b)
+ assert len(set(a[0].tolist())) == mb.n_active # без повторов
+
+
+def test_learning_suppresses_only_what_was_shown():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=4000, sparsity=0.01, seed=2))
+ rng = np.random.default_rng(3)
+ familiar = rng.normal(size=(200, mb.n_pn)).astype(np.float32)
+ novel = (rng.normal(size=(50, mb.n_pn)) + 8.0).astype(np.float32)
+ mb.fit_normalizer(familiar)
+ mb.learn(familiar, rate=0.3)
+ assert mb.novelty(familiar).mean() < mb.novelty(novel).mean()
+
+
+def test_auto_rate_shrinks_with_sample_size():
+ mb = MushroomBody(MushroomBodyConfig(n_kc=10_000, sparsity=0.001))
+ assert mb.auto_rate(1_000) > mb.auto_rate(100_000)
+ assert 0 < mb.auto_rate(10 ** 7) <= 0.5
+
+
+def test_feature_count_mismatch_is_explicit():
+ mb = MushroomBody()
+ with pytest.raises(ValueError, match="признак"):
+ mb.encode(np.zeros((1, mb.n_pn + 1), np.float32))
+
+
+# --------------------------------------------------------------------------- мост в ROS
+
+class _FakeField:
+ def __init__(self, name, offset, datatype, count=1):
+ self.name, self.offset, self.datatype, self.count = name, offset, datatype, count
+
+
+class _FakeStamp:
+ def __init__(self, sec, nanosec):
+ self.sec, self.nanosec = sec, nanosec
+
+
+class _FakeHeader:
+ def __init__(self, stamp, frame_id):
+ self.stamp, self.frame_id = stamp, frame_id
+
+
+class _FakeMsg:
+ """Достаточная имитация sensor_msgs/PointCloud2 для проверки моста."""
+
+ def __init__(self, pts, fields, point_step):
+ self.header = _FakeHeader(_FakeStamp(1788354623, 110040384), "hesai_lidar")
+ self.height, self.width = 1, pts.shape[0]
+ self.point_step = point_step
+ self.is_dense = False
+ self.fields = fields
+ self.data = pts.tobytes()
+
+
+def test_ros_bridge_matches_offline_parser():
+ """Путь через rclpy обязан давать то же, что и офлайн-разбор CDR."""
+ from flyguard import ros_conv
+
+ fields_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(fields_spec, 26)
+ rng = np.random.default_rng(11)
+ pts = np.zeros(256, dt)
+ pts["x"] = rng.normal(size=256).astype(np.float32)
+ pts["y"] = rng.normal(size=256).astype(np.float32)
+ pts["z"] = rng.normal(size=256).astype(np.float32)
+ pts["intensity"] = rng.uniform(0, 255, 256).astype(np.float32)
+ pts["ring"] = np.arange(256, dtype=np.uint16) % 128
+
+ msg = _FakeMsg(pts, [_FakeField(*f) for f in fields_spec], 26)
+ got = ros_conv.from_ros(msg)
+
+ assert got.n_points == 256
+ assert got.frame_id == "hesai_lidar"
+ assert got.stamp == pytest.approx(1788354623.110040384, abs=1e-6)
+ for name in ("x", "y", "z", "intensity", "ring"):
+ assert np.array_equal(got.points[name], pts[name])
+
+
+def test_ros_bridge_sorts_fields_by_offset():
+ """Порядок полей в сообщении не гарантирован — мост обязан его нормализовать."""
+ from flyguard import ros_conv
+
+ fields_spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1)]
+ dt = point_dtype(fields_spec, 16)
+ pts = np.zeros(8, dt)
+ pts["z"] = np.arange(8, dtype=np.float32)
+ shuffled = [_FakeField(*f) for f in reversed(fields_spec)]
+ got = ros_conv.from_ros(_FakeMsg(pts, shuffled, 16))
+ assert np.array_equal(got.points["z"], pts["z"])
+
+
+def test_ros_bridge_reports_missing_fields():
+ from flyguard import ros_conv
+
+ msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
+ ok, why = ros_conv.has_required_fields(msg)
+ assert not ok and "intensity" in why
+
+
+# --------------------------------------------------------- фигура и фон, привыкание
+
+def _cand(**kw):
+ from flyguard.lobula import Candidate
+
+ base = dict(d=60.0, u=0.1, h=1.0, d_min=59.7, h_min=0.3, width=0.4, height=1.0,
+ depth=0.5, containment=0.9, n_rays=30, n_rings=6, n_cols=5,
+ gap=8.0, on=0.001, floor_deficit=0.2, shadow=0.2, inten=40.0,
+ az_deg=0.5, el_deg=-1.0, bbox=(10, 16, 20, 25))
+ base.update(kw)
+ return Candidate(**base)
+
+
+def test_contrast_split_keeps_step_and_drops_smooth_wall():
+ """Гладкая стена не даёт фигуры, ступенька на ней — даёт."""
+ from flyguard.lobula import cluster_by_depth, split_by_figure
+
+ # стена: дальность плавно растёт вдоль строки от 20 до 120 м
+ r = np.tile(np.linspace(20.0, 120.0, 200, dtype=np.float32), (12, 1))
+ mask = np.ones(r.shape, bool)
+ labels, n = cluster_by_depth(mask, r, col_reach=3)
+ assert n == 1 # стена связна целиком
+
+ # контраст гладкой стены равен нулю — резать нечего
+ flat = np.zeros_like(r)
+ out, n_out = split_by_figure(labels, n, r, flat, max_depth=15.0, thr=6.0)
+ assert n_out == n and out is labels
+
+ # предмет: ступенька, торчащая из стены на 10 м
+ gap = np.zeros_like(r)
+ gap[4:8, 90:98] = 10.0
+ out, n_out = split_by_figure(labels, n, r, gap, max_depth=15.0, thr=6.0)
+ kept = out[out > 0]
+ assert kept.size == 32 # ровно лучи ступеньки
+ assert np.unique(kept).size == 1 # и это одна компонента
+
+
+def test_habituation_suppresses_shape_repeating_at_different_places():
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
+ seen = []
+ s = 0.0
+ for _ in range(14):
+ s += 25.0
+ c = _cand(d=60.0)
+ hab.update([c], s, 25.0)
+ seen.append(c.novelty)
+ assert seen[0] > 0.9 and seen[-1] < 0.25
+ assert hab.places >= 10
+
+
+def test_habituation_keeps_object_standing_in_one_place_novel():
+ """Подъезд к неподвижному предмету — это одно место, а не сто повторов."""
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1))
+ s = 0.0
+ nov = []
+ for k in range(50):
+ s += 3.0
+ nov.append(_cand(d=160.0 - 3.0 * k))
+ hab.update([nov[-1]], s, 3.0)
+ assert hab.places <= 2 # одно место (плюс дрейф оценки)
+ assert nov[-1].novelty > 0.7
+
+
+def test_habituation_forgets_after_a_long_run():
+ from flyguard.mushroom_body import Habituation, HabituationConfig
+
+ hab = Habituation(HabituationConfig(warmup=0, norm_n=1, recover_m=100.0))
+ s = 0.0
+ for _ in range(14):
+ s += 25.0
+ hab.update([_cand(d=60.0)], s, 25.0)
+ low = hab.level
+ hab.advance(600.0)
+ assert hab.level < low * 0.2
+
+
+# --------------------------------------------------------------- считывание MBON
+
+def test_mbon_readout_learns_and_round_trips(tmp_path):
+ """Обучается, сохраняется без потерь и даёт калиброванную вероятность."""
+ from flyguard.mbon_readout import MbonConfig, MbonReadout
+
+ rng = np.random.default_rng(3)
+ X = rng.standard_normal((2000, 23)).astype(np.float32)
+ y = ((X[:, 0] + 0.5 * X[:, 3]) > 0.3).astype(np.float32)
+ m = MbonReadout(MbonConfig(n_kc=4000, sparsity=0.02), n_pn=23)
+ m.fit_normalizer(X)
+ m.learn(X, y, epochs=60, lr=4.0)
+
+ s = m.score(X)
+ assert s.min() >= 0.0 and s.max() <= 1.0
+ assert ((s > 0.5) == (y > 0.5)).mean() > 0.8 # калибровка, не только порядок
+
+ path = tmp_path / "mbon.npz"
+ m.save(path)
+ again = MbonReadout.load(path)
+ assert np.allclose(again.score(X[:64]), s[:64], atol=1e-6)
+
+
+def test_mbon_replaces_hand_formula_and_blend_interpolates():
+ """Модель входит в вес улики, а смешивание даёт обе крайности."""
+ from flyguard.central_complex import _quality
+
+ c = _cand(gap=0.0, containment=0.3, depth=9.0, h_min=1.4, n_rays=5)
+ hand = _quality(c) # без модели
+ c.extra["mbon"] = 0.99
+ assert _quality(c, mbon_blend=1.0) > hand * 3 # модель вытягивает
+ assert _quality(c, mbon_blend=0.0) == hand # ручная формула нетронута
+ mid = _quality(c, mbon_blend=0.5)
+ assert hand < mid < _quality(c, mbon_blend=1.0)
+
+ # уверенное «это обстановка» гасит даже хорошую геометрию
+ good = _cand(gap=12.0, containment=1.0, depth=0.4, h_min=0.3, n_rays=80)
+ strong = _quality(good)
+ good.extra["mbon"] = 0.01
+ assert _quality(good, mbon_blend=1.0) < strong * 0.2
+
+
+def test_mbon_absent_leaves_pipeline_unchanged():
+ """Без модели вес улики считается ровно как раньше."""
+ from flyguard.central_complex import _quality
+
+ c = _cand()
+ assert "mbon" not in c.extra
+ assert _quality(c, mbon_blend=1.0) == _quality(c, mbon_blend=0.0)
+
+
+def test_far_warning_threshold_only_relaxes_beyond_its_distance():
+ """Послабление далёким трекам не должно трогать ближнюю зону."""
+ from flyguard.descending import DescendingNeurons
+
+ flat = DescendingNeurons()
+ assert flat._warn_at(10.0) == flat._warn_at(199.0) == flat.warn_evidence
+
+ dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.3,
+ warn_far_from=90.0, max_range=200.0)
+ assert dn._warn_at(50.0) == 0.5 # ближняя зона не тронута
+ assert dn._warn_at(90.0) == 0.5 # ровно на границе — тоже
+ assert abs(dn._warn_at(145.0) - 0.4) < 1e-6 # середина
+ assert abs(dn._warn_at(200.0) - 0.3) < 1e-6 # дальний край
+ assert abs(dn._warn_at(400.0) - 0.3) < 1e-6 # и не ниже него
+
+
+def test_novelty_fades_with_range_and_is_intact_up_close():
+ """Знакомость должна работать вблизи и не мешать вдали."""
+ from flyguard.central_complex import _quality
+
+ near, far = _cand(), _cand()
+ near.d, far.d = 40.0, 160.0
+ near.novelty = far.novelty = 0.15 # «знакомо», как далёкий предмет
+
+ # вблизи гашения нет: знакомое остаётся придавленным
+ a = _quality(near, nov_fade_from=0.0)
+ b = _quality(near, nov_fade_from=90.0)
+ assert abs(a - b) < 1e-9
+
+ # вдали вклад знакомости снят целиком
+ c = _quality(far, nov_fade_from=0.0)
+ d = _quality(far, nov_fade_from=90.0, nov_fade_to=160.0)
+ assert d > c
+
+ # и в промежутке — частично
+ mid = _cand(); mid.d, mid.novelty = 125.0, 0.15
+ m = _quality(mid, nov_fade_from=90.0, nov_fade_to=160.0)
+ assert _quality(mid, nov_fade_from=0.0) < m < d
+
+
+# ------------------------------------------------------- модель сенсора: яркость
+
+def test_injected_intensity_copies_the_surroundings():
+ """Яркость вставки берётся из записи, а не назначается.
+
+ Абсолютной шкалы интенсивности в данных нет: медиана кандидатов по бэгам
+ 3…7, а в записи с настоящим предметом 23.5. Любое назначенное число делает
+ вставку опознаваемой по одной яркости — сначала как «тускло = предмет»
+ (ламбертова ρ·cosθ/r²), потом как «ярко = предмет» (постоянные 35).
+ Поэтому у функции нет аргументов ни дальности, ни отражения.
+ """
+ import inspect
+
+ from flyguard.synth import local_intensity
+
+ # отражения среди аргументов нет; дальность есть, но только как выбор
+ # полосы реальных возвратов для лучей, ушедших в пустоту
+ args = set(inspect.signature(local_intensity).parameters)
+ assert not args & {"reflectivity", "rho", "cos_inc"}
+
+ rng = np.random.default_rng(0)
+ prev = np.array([10.0, 20.0, 0.0, 0.0], np.float32) # два луча в пустоту
+ pool = np.full(50, 30.0, np.float32)
+ frame = np.full(10_000, 7.0, np.float32)
+ v = local_intensity(prev, pool, frame, rng)
+
+ assert 8.0 < v[0] < 13.0 and 16.0 < v[1] < 25.0 # взято с тех же лучей
+ assert 24.0 < v[2] < 38.0 and 24.0 < v[3] < 38.0 # взято из запаса
+
+ # запас пуст — остаётся кадр целиком
+ v2 = local_intensity(np.zeros(200, np.float32), np.zeros(0, np.float32),
+ frame, rng)
+ assert 5.5 < float(np.median(v2)) < 9.0
+
+ # без разложения окружения по дальности сама дальность ничего не меняет
+ a = local_intensity(prev, pool, frame, np.random.default_rng(3), d=20.0)
+ b = local_intensity(prev, pool, frame, np.random.default_rng(3), d=200.0)
+ assert np.allclose(a, b)
+
+
+# --------------------------------------------------------------------------- данные
+
+@pytest.mark.skipif(not DATA.exists(), reason="датасет не распакован")
+def test_real_bag_projects_without_angular_error():
+ """На реальном бэге выпрямленная решётка обязана описывать лучи точно."""
+ from flyguard.bag import Bag
+
+ bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
+ clouds = [pc for _, pc in bag.frames(start=2, stop=8)]
+ lay = ScanLayout.calibrate(clouds)
+ img = lay.project(clouds[-1])
+
+ assert img.shape == (lay.n_rings, lay.n_az)
+ assert 0.2 < img.valid.mean() < 0.9
+ r = img.r_near[img.valid]
+ assert r.min() > 0 and r.max() < 250
+ assert np.all(img.r_far[img.valid] >= img.r_near[img.valid] - 1e-3)
+
+
+# ------------------------------------------------------------ раскладка по бэгам
+
+_TOOLS = ROOT / "tools" # выгрузка: tools лежит рядом с тестами
+if not _TOOLS.exists():
+ _TOOLS = ROOT.parents[2] / "tools" # основной проект: ros2_ws/src/flyguard
+if str(_TOOLS) not in sys.path:
+ sys.path.insert(0, str(_TOOLS))
+
+import _parallel as _P # noqa: E402
+
+
+def _twice(x):
+ """Задача для проверки. Верхнего уровня: иначе её не передать в процесс."""
+ return x * 2
+
+
+def test_parallel_keeps_task_order_when_results_arrive_out_of_order():
+ """Считается по готовности, складывается по номеру задачи.
+
+ Ломается это незаметно и опасно: цифры остаются правдоподобными, просто
+ приписываются не тому бэгу. Поэтому проверяется не «столько же строк», а
+ что результат каждой задачи лёг на своё место.
+ """
+ tasks = list(range(7))
+ want = [(t, t * 2) for t in tasks]
+
+ seq = [None] * len(tasks)
+ for i, t, r, _ in _P.run(_twice, tasks, jobs=1):
+ seq[i] = (t, r)
+
+ par = [None] * len(tasks)
+ for i, t, r, _ in _P.run(_twice, tasks, jobs=4):
+ par[i] = (t, r)
+
+ assert seq == want
+ assert par == want
+
+
+def test_parallel_never_starts_more_processes_than_there_are_bags():
+ """Бэгов пять, и шестой процесс занять нечем."""
+ assert _P.resolve(0, 1) == 1 # одна задача — без пула вовсе
+ assert _P.resolve(8, 3) == 3 # просили больше, чем есть работы
+ assert _P.resolve(1, 5) == 1 # явная последовательная отладка
+ assert 1 <= _P.resolve(0, 5) <= 5
+
+
+# ------------------------------------------------------------ решение по треку
+
+def _pair_of_tracks():
+ """Два трека с ОДИНАКОВОЙ насыщенной уликой и разным средним весом."""
+ from flyguard.central_complex import CentralComplex, Track
+
+ cx = CentralComplex()
+ cx.s_world = 0.0
+ good = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
+ evidence=1.0, hits=9, age=9, novelty=0.6,
+ n_obs=9, w_sum=9 * 0.95, p_sum=9 * 0.95)
+ junk = Track(id=2, s_world=70.0, u=0.5, h=0.3, width=0.5, height=1.7,
+ evidence=1.0, hits=9, age=9, novelty=0.6,
+ n_obs=9, w_sum=9 * 0.27, p_sum=9 * 0.27)
+ cx.tracks = [good, junk]
+ return cx
+
+
+def test_saturated_evidence_stops_separating_and_the_mean_weight_does_not():
+ """Улика обрезана единицей — и перестаёт различать.
+
+ На реальном объекте медиана улики 1.000 и у предмета, и у ложных треков
+ (AUC 0.624), потому что сумма упёрлась в потолок у обоих. Средний вес
+ наблюдения при этом 0.998 против 0.269. Здесь это проверяется на паре
+ треков, у которых улика одинакова НАРОЧНО.
+ """
+ from flyguard.descending import DescendingNeurons
+
+ plain = DescendingNeurons().decide(_pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in plain.objects} == {1, 2}
+
+ picked = DescendingNeurons(track_blend=0.7, track_score="w_mean").decide(
+ _pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in picked.objects} == {1}
+
+
+def test_track_score_without_a_model_falls_back_to_evidence():
+ """`model` без файла модели обязан вести себя как раньше, а не как ноль.
+
+ Отсчёт модели по умолчанию нулевой, и молчаливое его использование
+ погасило бы вообще все треки.
+ """
+ from flyguard.descending import DescendingNeurons
+
+ dn = DescendingNeurons(track_blend=0.7, track_score="model",
+ track_readout=None)
+ out = dn.decide(_pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in out.objects} == {1, 2}
+
+
+def test_track_accumulators_do_not_touch_the_evidence():
+ """Сводка по треку копится отдельно и в улику не вмешивается."""
+ from flyguard.central_complex import CentralComplex
+ from flyguard.lobula import Candidate
+
+ cx = CentralComplex()
+ c = Candidate(d=60.0, u=0.0, h=0.5, d_min=59.8, h_min=0.1, width=0.4,
+ height=1.6, depth=0.3, containment=0.9, n_rays=40,
+ n_rings=8, n_cols=6, gap=4.0, on=0.2, floor_deficit=0.0,
+ shadow=0.3, inten=12.0, az_deg=0.0, el_deg=-1.0,
+ bbox=(0, 0, 8, 6), novelty=0.5)
+ cx.update([c], ds=0.0, dt=0.1)
+ t = cx.tracks[0]
+ assert t.n_obs == 1 and t.w_mean > 0.0
+ assert abs(t.evidence - cx.gain * t.w_mean) < 1e-6
+
+
+def test_track_p_mean_falls_back_when_there_is_no_readout():
+ """Без модели покадрового считывания отсчёта нет, и ноль тут погасил бы всё."""
+ from flyguard.central_complex import Track
+
+ t = Track(id=1, s_world=60.0, u=0.0, h=0.3, width=0.5, height=1.7,
+ n_obs=4, w_sum=4 * 0.8)
+ assert t.p_n == 0
+ assert abs(t.p_mean - 0.8) < 1e-9
+ t.p_n, t.p_sum = 4, 4 * 0.3
+ assert abs(t.p_mean - 0.3) < 1e-9
+
+
+def test_track_gate_removes_the_weak_track_without_touching_the_strong_one():
+ """Порог ничего не отнимает у прошедшего — в отличие от смешивания."""
+ from flyguard.descending import DescendingNeurons
+
+ out = DescendingNeurons(track_gate=0.5, track_score="w_mean").decide(
+ _pair_of_tracks(), speed=10.0)
+ assert {o.track_id for o in out.objects} == {1}
+ assert out.objects[0].confidence == pytest.approx(
+ DescendingNeurons().decide(_pair_of_tracks(), speed=10.0).objects[0].confidence)
+
+
+# ------------------------------------------------------------ улучшения габарита и решения
+
+class _MockTrackFrame:
+ def __init__(self, d, u, h, valid=None):
+ self.d = np.asarray(d, np.float32)
+ self.u = np.asarray(u, np.float32)
+ self.h = np.asarray(h, np.float32)
+ self.valid = np.ones_like(self.d, dtype=bool) if valid is None else np.asarray(valid, bool)
+
+ def lateral(self, corridor=None):
+ if corridor is None or corridor.n_slices == 0:
+ return self.u
+ curved = self.u - corridor.centre(self.d)
+ return np.where(np.abs(curved) < np.abs(self.u), curved, self.u)
+
+
+def test_gauge_mask_zonal_floor_and_corridor_expansion():
+ """Проверка зонального пола и расширения габарита по неопределённости оси."""
+ from flyguard.geometry import STRAIGHT, Corridor
+ from flyguard.lobula import gauge_mask
+
+ # 1. Зональный пол: в колее (|u| <= 0.85) порог 0.16 м, снаружи — 0.28 м
+ tf = _MockTrackFrame(
+ d=[50.0, 50.0],
+ u=[0.4, 1.2],
+ h=[0.20, 0.20] # оба 20 см над рельсом
+ )
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, h_lo_core=0.16, core_width=0.85)
+ assert inside[0] # в колее 0.20 м > 0.16 м — внутри габарита!
+ assert not inside[1] # на периферии 0.20 м < 0.28 м — отсечено!
+
+ # 2. Расширение габарита на дальней дистанции при кривизне
+ corridor = Corridor(np.array([0.0, 0.0, 0.0003], np.float32), 60.0, 10, 1600.0)
+ tf_far = _MockTrackFrame(
+ d=[150.0],
+ u=[1.90],
+ h=[1.0]
+ )
+ # Без расширения (k_sigma=0): |u| = 1.9 > 1.6 -> отсечено
+ inside_rigid, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, k_sigma=0.0)
+ assert not inside_rigid[0]
+
+ # С расширением (k_sigma=0.75): w_eff > 1.9 м -> объект удержан
+ inside_exp, _, _ = gauge_mask(tf_far, corridor, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, k_sigma=0.75)
+ assert inside_exp[0]
+
+
+def test_core_floor_is_not_lowered_closer_than_core_from():
+ """Вблизи опущенный пол открывает головки рельсов — там он не действует."""
+ from flyguard.geometry import STRAIGHT
+ from flyguard.lobula import gauge_mask
+
+ tf = _MockTrackFrame(d=[20.0, 60.0, 60.0], u=[0.4, 0.4, 1.2], h=[0.20, 0.20, 0.20])
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, h_lo_core=0.16,
+ core_width=0.85, core_from=55.0)
+ assert not inside[0] # 20 м — ближе порога, пол прежний
+ assert inside[1] # 60 м в колее — пол опущен
+ assert not inside[2] # вне колеи пол не опускается нигде
+
+
+def test_gauge_mask_filters_platform_surface():
+ """Настил платформы (h~1.1-1.2 м, |u| >= 1.30 м) отсекается, а препятствие в габарите остаётся."""
+ from flyguard.geometry import STRAIGHT
+ from flyguard.lobula import gauge_mask
+
+ tf = _MockTrackFrame(
+ d=[40.0, 40.0],
+ u=[1.40, 0.90],
+ h=[1.15, 1.15]
+ )
+ inside, _, _ = gauge_mask(tf, STRAIGHT, half_width=1.6, h_lo=0.28, h_hi=2.3,
+ d_min=4.0, d_max=220.0, platform_filter=True)
+ assert not inside[0] # настил платформы отсечён
+ assert inside[1] # препятствие на колее сохранено
+
+
+def test_descending_far_soft_warning_triggers_without_emergency():
+ """На 130 м устойчивый трек поднимает предупреждение, но не экстренное торможение."""
+ from flyguard.central_complex import CentralComplex, Track
+ from flyguard.descending import DescendingNeurons
+
+ cx = CentralComplex()
+ cx.s_world = 0.0
+ far_track = Track(
+ id=1, s_world=130.0, u=0.2, h=0.5, width=0.5, height=1.6,
+ evidence=0.20, hits=3, age=5, novelty=0.12,
+ n_obs=3, w_sum=3 * 0.85, p_sum=3 * 0.85
+ )
+ cx.tracks = [far_track]
+
+ dn = DescendingNeurons(warn_evidence=0.5, warn_far=0.35, warn_far_from=90.0,
+ far_channel=True)
+ decision = dn.decide(cx, speed=15.0)
+
+ assert decision.detected is True
+ assert decision.emergency is False
+ assert decision.distance == pytest.approx(130.0, abs=1.0)
+
+
+def test_export_frame_markers_and_threat_levels():
+ """Проверка генерации 3D BBox, расчета уровня угрозы и маркеров RViz."""
+ from flyguard.descending import Decision, DetectedObject
+ from flyguard.export import ThreatLevel, export_frame
+ from flyguard.geometry import RailPlane, STRAIGHT
+
+ plane = RailPlane(a=0.01, b=0.0, c=-1.80, inliers=100, rms=0.02)
+ dec = Decision(
+ detected=True,
+ emergency=False,
+ distance=45.0,
+ ttc=4.5,
+ speed=10.0,
+ stopping_distance=50.0,
+ objects=[
+ DetectedObject(
+ distance=45.0, lateral=0.3, height=0.6, width=0.5, size_v=1.2,
+ confidence=0.92, novelty=0.8, n_rays=15, track_id=42, ttc=4.5
+ )
+ ]
+ )
+
+ exp = export_frame(dec, plane, STRAIGHT, stamp=1710000000.5)
+ assert exp.threat_level == ThreatLevel.WARNING
+ assert len(exp.boxes) == 1
+ b = exp.boxes[0]
+ assert b.track_id == 42
+ assert b.distance_along_track == 45.0
+ assert b.x == pytest.approx(0.3)
+ assert b.y == pytest.approx(-45.0)
+ # z = h + a*d + b*u + c = 0.6 + 0.01*45 + 0.0 + (-1.8) = -0.75
+ assert b.z == pytest.approx(0.6 + 0.01 * 45.0 - 1.80)
+
+ # Проверка сериализации в словарь и RViz маркеры
+ d = exp.to_dict()
+ assert d["threat_level"] == "WARNING"
+ assert d["n_objects"] == 1
+
+ markers = exp.to_rviz_markers(frame_id="hesai_pandar")
+ assert len(markers) >= 2 # bbox cube + text label
+
+
+def test_benchmark_augmentation_logic():
+ """Проверка генерации сценариев и логики аугментаций в make_benchmark."""
+ from make_benchmark import _ScenarioState
+ from flyguard.pipeline import FlyGuard, Params
+
+ rng = np.random.default_rng(42)
+ p = FlyGuard(Params())
+
+ # 1. Спавн в диапазоне [40, 200] м
+ d_start_min, d_start_max = 40.0, 200.0
+ spawns = [float(rng.uniform(d_start_min, d_start_max)) for _ in range(100)]
+ assert all(40.0 <= d <= 200.0 for d in spawns)
+ assert min(spawns) < 60.0 and max(spawns) > 180.0
+
+ # 2. Боковой дрейф для пешеходов
+ v_lat_max = 0.35
+ v_lats = [float(rng.uniform(-v_lat_max, v_lat_max)) for _ in range(100)]
+ assert all(-0.35 <= v <= 0.35 for v in v_lats)
+
+ # 3. Шум по дальности
+ s_noise_std = 0.12
+ noises = [float(np.clip(rng.normal(0.0, s_noise_std), -0.35, 0.35)) for _ in range(500)]
+ assert abs(np.mean(noises)) < 0.05
+ assert 0.08 < np.std(noises) < 0.15
+
+ # 4. _ScenarioState
+ sc = _ScenarioState(name="человек_лежа", lat_base=0.0, pipe=p,
+ d_init=120.0, v_lat=0.15, is_static=False)
+ assert sc.name == "человек_лежа"
+ assert sc.v_lat == 0.15
+ assert not sc.is_static
+
+
+def test_device_detection_and_fallback():
+ """Проверка автоматического определения GPU устройства и fallback на CPU."""
+ from flyguard.device import get_device, get_device_info, is_cuda_available, to_numpy
+
+ dev = get_device("auto")
+ assert dev in ("cpu", "cuda") or dev.startswith("cuda:")
+
+ if not is_cuda_available():
+ assert get_device("cuda") == "cpu"
+ assert get_device("cpu") == "cpu"
+
+ info = get_device_info()
+ assert "device" in info
+ assert "torch_available" in info
+
+ arr = np.array([1.0, 2.0, 3.0], dtype=np.float32)
+ assert np.array_equal(to_numpy(arr), arr)
+
+
+def test_lamina_device_routing():
+ """Проверка работы Lamina с указанием устройства (cpu / auto)."""
+ from flyguard import lamina
+
+ r = np.full((16, 32), 20.0, dtype=np.float32)
+ r[8, 16] = 5.0 # препятствие
+ valid = np.ones((16, 32), dtype=bool)
+
+ out_cpu = lamina.process(r, valid, device="cpu")
+ out_auto = lamina.process(r, valid, device="auto")
+
+ assert out_cpu.on.shape == (16, 32)
+ assert out_auto.on.shape == (16, 32)
+ assert out_cpu.on[8, 16] > 0.0
+ assert np.allclose(out_cpu.on, out_auto.on, atol=1e-5)
+
+
+def test_device_runtime_failure_and_fallback():
+ """Проверка динамического перехода на CPU при сбое/отвале GPU в рантайме."""
+ from flyguard.device import is_cuda_available, get_device, notify_cuda_error, reset_device_cache
+ from flyguard import lamina
+
+ # Симуляция критического сбоя GPU
+ notify_cuda_error(RuntimeError("Simulated CUDA device disconnect / OOM"))
+ try:
+ assert not is_cuda_available()
+ assert get_device("cuda") == "cpu"
+ assert get_device("auto") == "cpu"
+
+ r = np.full((16, 32), 20.0, dtype=np.float32)
+ r[8, 16] = 4.0
+ valid = np.ones((16, 32), dtype=bool)
+
+ # Даже при явном указании device="cuda", Lamina должна успешно отработать на CPU
+ out = lamina.process(r, valid, device="cuda")
+ assert out.on.shape == (16, 32)
+ assert out.on[8, 16] > 0.0
+ finally:
+ reset_device_cache()
+
+
+
+def test_scenario_stream_does_not_depend_on_the_rest_of_the_catalogue():
+ """Новый предмет в каталоге не сдвигает случайность остальных сценариев.
+
+ Раньше генератор был один на запись, и сценарии тянули из него по очереди:
+ добавили человека лёжа — поменялись цифры по ящику. Теперь поток сценария
+ определяется только зерном, именем предмета и поперечным смещением.
+ """
+ from make_benchmark import scenario_rng
+
+ a = scenario_rng(12345, "ящик", 0.9).random(5)
+ assert np.array_equal(a, scenario_rng(12345, "ящик", 0.9).random(5))
+ assert not np.array_equal(a, scenario_rng(12345, "ящик", 0.0).random(5))
+ assert not np.array_equal(a, scenario_rng(12345, "чемодан", 0.9).random(5))
+ assert not np.array_equal(a, scenario_rng(777, "ящик", 0.9).random(5))
+
+
+def test_export_box_follows_a_curved_track():
+ """В кривой рамка стоит на пути и повёрнута вдоль него.
+
+ Боковое смещение трека отсчитано от оси пути, и без поправки на её
+ положение рамка на 150 м при радиусе 1300 м стояла в 8.6 м от пути, а
+ поворот был зеркальным — 13° мимо направления пути.
+ """
+ import math
+ from flyguard.descending import Decision, DetectedObject
+ from flyguard.export import export_frame
+ from flyguard.geometry import Corridor, RailPlane
+
+ R = 1300.0
+ cor = Corridor(np.array([0.0, 0.0, 1.0 / (2 * R)], np.float32), 200.0, 10, 1600.0)
+ plane = RailPlane(a=0.0, b=0.0, c=-1.8, inliers=100, rms=0.02)
+ for d in (55.0, 100.0, 150.0):
+ dec = Decision(detected=True, distance=d, speed=10.0, stopping_distance=30.0,
+ objects=[DetectedObject(distance=d, lateral=0.3, height=0.8,
+ width=0.5, size_v=1.7, confidence=0.9,
+ novelty=0.6, n_rays=10, track_id=1,
+ ttc=d / 10)])
+ box = export_frame(dec, plane, cor).boxes[0]
+ on_track = float(cor.centre(np.array([d], np.float32))[0]) + 0.3
+ assert box.x == pytest.approx(on_track, abs=1e-3)
+
+ slope = d / R
+ long_axis = np.array([-math.sin(box.yaw), math.cos(box.yaw)])
+ tangent = np.array([slope, -1.0]) / math.hypot(slope, 1.0)
+ assert abs(float(long_axis @ tangent)) == pytest.approx(1.0, abs=1e-6)
+
+
+def test_track_carries_the_sensor_lateral_for_drawing():
+ """Рамка в RViz стоит у своих точек, а не у оси пути.
+
+ На кривой синтетики организаторов (радиус около 760 м) предмет на оси
+ пути на 39 м лежит в 1.05 м левее оси лидара, а рамка рисовалась по
+ смещению от оси пути — на +0.13 м, в 1.2 м от своих точек.
+ """
+ from flyguard.central_complex import CentralComplex
+ from flyguard.descending import DescendingNeurons
+ from flyguard.lobula import Candidate
+
+ cx = CentralComplex()
+ c = Candidate(d=39.0, u=0.02, h=0.3, d_min=38.8, h_min=0.1, width=0.3,
+ height=0.3, depth=0.2, containment=1.0, n_rays=20,
+ n_rings=5, n_cols=4, gap=6.0, on=0.2, floor_deficit=0.0,
+ shadow=0.3, inten=12.0, az_deg=-1.5, el_deg=-1.0,
+ bbox=(0, 0, 5, 4), novelty=0.6, extra={"u_raw": -1.05})
+ cx.update([c], ds=0.0, dt=0.1)
+ t = cx.tracks[0]
+ assert t.u == pytest.approx(0.02) and t.u_raw == pytest.approx(-1.05)
+
+ t.evidence, t.hits, t.age = 1.0, 9, 9
+ obj = DescendingNeurons().decide(cx, speed=10.0).objects[0]
+ assert obj.lateral == pytest.approx(0.02) # смещение от оси пути — в сообщении
+ assert obj.sensor_x == pytest.approx(-1.05) # а рисуется рамка здесь
+
+
+def test_gauge_outline_bends_with_the_track():
+ """Контур габарита в RViz идёт вдоль оценённой оси, а не прямо."""
+ from flyguard.export import gauge_outline
+ from flyguard.geometry import Corridor
+
+ boxes = [(1.2, 0.28, 2.3, 4.0, 200.0)]
+ straight = np.array(gauge_outline(boxes))
+ assert set(np.round(np.abs(straight[:, 0]), 6)) == {1.2}
+
+ R = 760.0
+ cor = Corridor(np.array([0.0, 0.0, -1.0 / (2 * R)], np.float32), 200.0, 10, R)
+ bent = np.array(gauge_outline(boxes, cor.centre))
+ at100 = bent[np.isclose(bent[:, 1], -100.0)]
+ c100 = float(cor.centre(np.array([100.0], np.float32))[0])
+ assert c100 == pytest.approx(-100.0 ** 2 / (2 * R), rel=1e-3)
+ assert at100[:, 0].min() == pytest.approx(c100 - 1.2, abs=1e-4)
+ assert at100[:, 0].max() == pytest.approx(c100 + 1.2, abs=1e-4)
+
+
+# ------------------------------------------------------------ видеокарта
+
+def _cuda_or_skip():
+ torch = pytest.importorskip("torch")
+ if not torch.cuda.is_available():
+ pytest.skip("нет видеокарты с CUDA")
+
+
+def _scene(seed=0, shape=(128, 600)):
+ """Образ, похожий на тоннель: гладкие стены, ступеньки-предметы, пропуски эха."""
+ rng = np.random.default_rng(seed)
+ h, w = shape
+ r = (np.linspace(3.0, 180.0, w)[None, :] * (1.0 + 0.2 * np.sin(np.arange(h)[:, None] / 9.0))
+ ).astype(np.float32)
+ for _ in range(40):
+ i, j = rng.integers(0, h - 8), rng.integers(0, w - 12)
+ r[i:i + rng.integers(2, 8), j:j + rng.integers(2, 12)] *= np.float32(rng.uniform(0.3, 0.9))
+ r += rng.normal(0.0, 0.02, r.shape).astype(np.float32)
+ valid = rng.random(shape) > 0.1
+ return np.where(valid, r, np.float32(0.0)), valid
+
+
+def test_gpu_lamina_and_clustering_match_cpu_bit_for_bit():
+ """Ламина и кластеризация на видеокарте — те же числа и те же номера компонент."""
+ _cuda_or_skip()
+ import torch
+ from flyguard import lamina
+ from flyguard.gpu import GpuStages
+ from flyguard.lobula import cluster_by_depth
+
+ g = GpuStages("cuda")
+ for seed in range(3):
+ r, valid = _scene(seed)
+ g.r = torch.as_tensor(r, device="cuda")
+ g.valid = torch.as_tensor(valid, device="cuda")
+ a, b = lamina._process_cpu(r, valid), g.lamina()
+ for f in ("disp", "on", "off", "on_scale", "surround", "hole"):
+ assert np.array_equal(getattr(a, f), getattr(b, f)), f
+ mask = valid & (np.random.default_rng(seed).random(r.shape) < 0.5)
+ la, na = cluster_by_depth(mask, r)
+ lb, nb = g.cluster_by_depth(mask, r)
+ assert na == nb and np.array_equal(la, lb)
+
+
+def test_gpu_projection_matches_cpu():
+ """Сетчатка на видеокарте: целый кадр — бит в бит, перемешанный — почти."""
+ _cuda_or_skip()
+ from flyguard.bag import Bag
+ from flyguard.cdr import PointCloud2
+ from flyguard.gpu import GpuStages
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ bag = Bag(next(p for p in DATA.iterdir() if p.is_dir()))
+ clouds = [pc for _, pc in bag.frames(start=2, stop=16)]
+ lay = ScanLayout.calibrate(clouds[:12])
+ cols = lay.column_slice(30.0)
+ g = GpuStages("cuda")
+ pc = clouds[-1]
+ a, b = lay.project(pc, cols), g.project(lay, pc, cols)
+ for f in ("r_near", "r_far", "inten", "valid"):
+ assert np.array_equal(getattr(a, f), getattr(b, f)), f
+
+ # порядок точек сбит, как в синтетике организаторов: раскладка по углам
+ rng = np.random.default_rng(0)
+ mixed = PointCloud2(pc.stamp, pc.frame_id, 1, pc.n_points, pc.point_step,
+ pc.is_dense, pc.points[rng.permutation(pc.n_points)])
+ a, b = lay.project(mixed, cols), g.project(lay, mixed, cols)
+ assert a.valid.sum() > 1000
+ differ = sum(int((getattr(a, f) != getattr(b, f)).sum())
+ for f in ("r_near", "r_far", "inten", "valid"))
+ assert differ < 1e-3 * a.valid.size # последний знак арктангенса и порядок равных
+
+
+def test_pipeline_survives_gpu_failure_mid_frame():
+ """Отказ видеокарты посреди кадра: кадр досчитан на процессоре, решение то же."""
+ from flyguard.bag import Bag
+ from flyguard.device import reset_device_cache
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+
+ class Broken:
+ def project(self, *a, **k):
+ raise RuntimeError("CUDA error: device lost (имитация)")
+
+ lamina = cluster_by_depth = project
+
+ frames = [pc for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=30)]
+ ref = FlyGuard(Params(device="cpu"))
+ fg = FlyGuard(Params(device="cpu"))
+ try:
+ for i, pc in enumerate(frames):
+ if i == 20: # видеокарта «была» и отказала
+ fg.gpu, fg.device = Broken(), "cuda"
+ a, b = ref.process(pc), fg.process(pc)
+ assert (a is None) == (b is None)
+ if a is not None:
+ assert a.decision.detected == b.decision.detected
+ assert [c.d for c in a.candidates] == [c.d for c in b.candidates]
+ assert fg.gpu is None and fg.device == "cpu"
+ assert "device lost" in fg.gpu_error
+ finally:
+ reset_device_cache()
+
+
+class _CpuStages:
+ """Подмена видеокарты: те же стадии на процессоре, прогрев ждёт сигнала."""
+
+ gate = None
+
+ def __init__(self, device):
+ self.calls = 0
+ self._img = None
+
+ def warmup(self):
+ if self.gate is not None:
+ self.gate.wait()
+
+ def project(self, L, pc, cols):
+ self.calls += 1
+ self._img = L.project(pc, cols)
+ return self._img
+
+ def lamina(self):
+ from flyguard import lamina
+ return lamina.process(self._img.r_near, self._img.valid, device="cpu")
+
+ def cluster_by_depth(self, mask, r, **kw):
+ return cluster_by_depth(mask, r, **kw)
+
+
+def _wait_gpu(fg, timeout=5.0):
+ import time
+ t_end = time.monotonic() + timeout
+ while fg.gpu_pending and time.monotonic() < t_end:
+ time.sleep(0.01)
+ assert not fg.gpu_pending, "поток видеокарты не закончил"
+
+
+def test_gpu_comes_up_in_background_while_cpu_counts(monkeypatch):
+ """Прогрев видеокарты идёт в фоне: кадры тем временем считает процессор,
+ а на видеокарту конвейер переходит только с начала кадра — с тем же итогом.
+ Зависший прогрев (здесь — до 20-го кадра) узел не держит."""
+ import threading
+ import flyguard.device as device
+ import flyguard.gpu as gpu
+ from flyguard.bag import Bag
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ made = []
+
+ class Stages(_CpuStages):
+ gate = threading.Event()
+
+ def __init__(self, dev):
+ super().__init__(dev)
+ made.append(self)
+
+ monkeypatch.setattr(device, "get_device", lambda preferred="auto": "cuda")
+ monkeypatch.setattr(gpu, "GpuStages", Stages)
+ frames = [pc for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=30)]
+ ref = FlyGuard(Params(device="cpu"))
+ fg = FlyGuard(Params(device="auto"), gpu_background=True)
+ try:
+ for i, pc in enumerate(frames):
+ if i == 20:
+ assert fg.gpu_pending and fg.gpu is None and fg.device == "cpu"
+ Stages.gate.set()
+ _wait_gpu(fg)
+ assert fg.gpu_active and fg.gpu is None # готова, но ждёт начала кадра
+ a, b = ref.process(pc), fg.process(pc)
+ assert (a is None) == (b is None)
+ if a is not None:
+ assert a.decision.detected == b.decision.detected
+ assert [c.d for c in a.candidates] == [c.d for c in b.candidates]
+ assert fg.gpu is made[0] and fg.device == "cuda"
+ assert made[0].calls == len(frames) - 20
+ finally:
+ Stages.gate.set()
+
+
+def test_gpu_failing_to_warm_up_leaves_cpu(monkeypatch):
+ """Прогрев в фоне упал: конвейер остаётся на процессоре, причина — в gpu_error."""
+ import flyguard.device as device
+ import flyguard.gpu as gpu
+ from flyguard.device import reset_device_cache
+ from flyguard.pipeline import FlyGuard, Params
+
+ class Bad(_CpuStages):
+ def warmup(self):
+ raise RuntimeError("CUDA error: no kernel image is available (имитация)")
+
+ monkeypatch.setattr(device, "get_device", lambda preferred="auto": "cuda")
+ monkeypatch.setattr(gpu, "GpuStages", Bad)
+ try:
+ fg = FlyGuard(Params(device="auto"), gpu_background=True)
+ _wait_gpu(fg)
+ assert fg.gpu is None and not fg.gpu_active and fg.device == "cpu"
+ assert "no kernel image" in fg.gpu_error
+ finally:
+ reset_device_cache()
+
+
+def test_brain_view_scales_for_high_resolution():
+ """Мозг для экрана в 2×: вдвое больше по обеим осям, облако из атласа
+ своего размера, а в целом та же картинка, что у узла."""
+ cv2 = pytest.importorskip("cv2")
+ # в выгрузке для команды вида мозга нет — там тест пропускается
+ BrainHybrid = pytest.importorskip("flyguard.brain_hybrid").BrainHybrid
+ from flyguard.bag import Bag
+ from flyguard.pipeline import FlyGuard, Params
+
+ if not DATA.exists():
+ pytest.skip("нет записей")
+ one, two = BrainHybrid(), BrainHybrid(scale=2)
+ if not one.enabled:
+ pytest.skip("нет атласа нейронов")
+ fg = FlyGuard(Params(enable_looming=True))
+ a = b = None
+ for _, pc in Bag(next(p for p in DATA.iterdir() if p.is_dir())).frames(stop=20):
+ res = fg.process(pc, keep_debug=True)
+ if res is not None:
+ a, b = one.render(res), two.render(res)
+ assert a is not None
+ assert b.shape == (2 * a.shape[0], 2 * a.shape[1], 3)
+ small = cv2.resize(b, (a.shape[1], a.shape[0]), interpolation=cv2.INTER_AREA)
+ assert abs(float(small.mean()) - float(a.mean())) < 0.15 * float(a.mean())
diff --git a/ros2_ws/src/flyguard_msgs/CMakeLists.txt b/ros2_ws/src/flyguard_msgs/CMakeLists.txt
new file mode 100644
index 0000000..0609928
--- /dev/null
+++ b/ros2_ws/src/flyguard_msgs/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(flyguard_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/DetectedObject.msg"
+ "msg/ObstacleStatus.msg"
+ DEPENDENCIES std_msgs
+)
+
+ament_package()
diff --git a/ros2_ws/src/flyguard_msgs/msg/DetectedObject.msg b/ros2_ws/src/flyguard_msgs/msg/DetectedObject.msg
new file mode 100644
index 0000000..eb33144
--- /dev/null
+++ b/ros2_ws/src/flyguard_msgs/msg/DetectedObject.msg
@@ -0,0 +1,12 @@
+# Один обнаруженный посторонний объект в габарите приближения поезда.
+
+float32 distance # расстояние вперёд по пути, м
+float32 lateral # смещение от оси пути, м (положительное вправо)
+float32 height # высота нижней точки над головкой рельса, м
+float32 width # поперечный размер, м
+float32 size_v # вертикальный размер, м
+float32 confidence # уверенность, 0..1
+float32 novelty # ответ MBON: 1 — незнакомая форма, 0 — штатная конструкция
+int32 n_rays # сколько лучей лидара попало в объект
+int32 track_id # идентификатор трека, стабилен между кадрами
+float32 time_to_collision # время до контакта при текущей скорости, с (inf, если стоим)
diff --git a/ros2_ws/src/flyguard_msgs/msg/ObstacleStatus.msg b/ros2_ws/src/flyguard_msgs/msg/ObstacleStatus.msg
new file mode 100644
index 0000000..132c677
--- /dev/null
+++ b/ros2_ws/src/flyguard_msgs/msg/ObstacleStatus.msg
@@ -0,0 +1,17 @@
+# Состояние пути перед беспилотным поездом за один лидарный кадр.
+#
+# Минимально необходимое для бортовой системы — поля detected и distance:
+# «путь свободен» либо «впереди препятствие на N метров».
+
+std_msgs/Header header
+
+bool detected # есть ли посторонний объект в габарите
+bool emergency # объект близко и подтверждён надёжно: экстренное торможение
+float32 distance # до ближайшего объекта, м (inf, если путь свободен)
+float32 time_to_collision # до ближайшего объекта, с
+float32 confidence # уверенность по ближайшему объекту, 0..1
+float32 speed # оценка скорости поезда по лидару, м/с (одометрии нет)
+float32 stopping_distance # требуемый тормозной путь при текущей скорости, м
+float32 processing_ms # время обработки кадра, мс
+
+DetectedObject[] objects # все подтверждённые объекты, ближний первым
diff --git a/ros2_ws/src/flyguard_msgs/package.xml b/ros2_ws/src/flyguard_msgs/package.xml
new file mode 100644
index 0000000..a3aa6f1
--- /dev/null
+++ b/ros2_ws/src/flyguard_msgs/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ flyguard_msgs
+ 1.0.0
+ Сообщения FlyGuard: статус пути перед беспилотным поездом.
+ FlyGuard
+ MIT
+
+ ament_cmake
+ rosidl_default_generators
+
+ std_msgs
+
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/tests/test_pipeline.py b/tests/test_pipeline.py
index a2af025..f32ac43 100644
--- a/tests/test_pipeline.py
+++ b/tests/test_pipeline.py
@@ -4,7 +4,7 @@
выпрямление скоса каналов, геометрия плоскости пути, связность с учётом
глубины, кодирование грибовидного тела и вставка синтетического предмета.
- pytest ros2_ws/src/flyguard/test
+ pytest tests
"""
from __future__ import annotations
@@ -209,6 +209,81 @@ def test_feature_count_mismatch_is_explicit():
mb.encode(np.zeros((1, mb.n_pn + 1), np.float32))
+# --------------------------------------------------------------------------- мост в ROS
+
+class _FakeField:
+ def __init__(self, name, offset, datatype, count=1):
+ self.name, self.offset, self.datatype, self.count = name, offset, datatype, count
+
+
+class _FakeStamp:
+ def __init__(self, sec, nanosec):
+ self.sec, self.nanosec = sec, nanosec
+
+
+class _FakeHeader:
+ def __init__(self, stamp, frame_id):
+ self.stamp, self.frame_id = stamp, frame_id
+
+
+class _FakeMsg:
+ """Достаточная имитация sensor_msgs/PointCloud2 для проверки моста."""
+
+ def __init__(self, pts, fields, point_step):
+ self.header = _FakeHeader(_FakeStamp(1788354623, 110040384), "hesai_lidar")
+ self.height, self.width = 1, pts.shape[0]
+ self.point_step = point_step
+ self.is_dense = False
+ self.fields = fields
+ self.data = pts.tobytes()
+
+
+def test_ros_bridge_matches_offline_parser():
+ """Путь через rclpy обязан давать то же, что и офлайн-разбор CDR."""
+ from flyguard import ros_conv
+
+ fields_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(fields_spec, 26)
+ rng = np.random.default_rng(11)
+ pts = np.zeros(256, dt)
+ pts["x"] = rng.normal(size=256).astype(np.float32)
+ pts["y"] = rng.normal(size=256).astype(np.float32)
+ pts["z"] = rng.normal(size=256).astype(np.float32)
+ pts["intensity"] = rng.uniform(0, 255, 256).astype(np.float32)
+ pts["ring"] = np.arange(256, dtype=np.uint16) % 128
+
+ msg = _FakeMsg(pts, [_FakeField(*f) for f in fields_spec], 26)
+ got = ros_conv.from_ros(msg)
+
+ assert got.n_points == 256
+ assert got.frame_id == "hesai_lidar"
+ assert got.stamp == pytest.approx(1788354623.110040384, abs=1e-6)
+ for name in ("x", "y", "z", "intensity", "ring"):
+ assert np.array_equal(got.points[name], pts[name])
+
+
+def test_ros_bridge_sorts_fields_by_offset():
+ """Порядок полей в сообщении не гарантирован — мост обязан его нормализовать."""
+ from flyguard import ros_conv
+
+ fields_spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1)]
+ dt = point_dtype(fields_spec, 16)
+ pts = np.zeros(8, dt)
+ pts["z"] = np.arange(8, dtype=np.float32)
+ shuffled = [_FakeField(*f) for f in reversed(fields_spec)]
+ got = ros_conv.from_ros(_FakeMsg(pts, shuffled, 16))
+ assert np.array_equal(got.points["z"], pts["z"])
+
+
+def test_ros_bridge_reports_missing_fields():
+ from flyguard import ros_conv
+
+ msg = _FakeMsg(np.zeros(4, np.dtype([("x", "f4")])), [_FakeField("x", 0, 7, 1)], 4)
+ ok, why = ros_conv.has_required_fields(msg)
+ assert not ok and "intensity" in why
+
+
# --------------------------------------------------------- фигура и фон, привыкание
def _cand(**kw):
diff --git a/tools/_bootstrap.py b/tools/_bootstrap.py
index 955563c..66fabc4 100644
--- a/tools/_bootstrap.py
+++ b/tools/_bootstrap.py
@@ -8,9 +8,9 @@ from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
PKG = ROOT
# Записи лидара в репозиторий не кладутся (десятки гигабайт). По умолчанию
-# ищем их рядом: сначала внутри выгрузки, потом на уровень выше — так работает
-# и у того, кто держит выгрузку внутри основного проекта, и у того, кто
-# распаковал её отдельно. Переопределяется переменной FLYGUARD_DATA.
+# ищем их рядом: сначала внутри репозитория, потом на уровень выше — так
+# работает и у того, кто держит клон внутри основного проекта, и у того, кто
+# распаковал его отдельно. Переопределяется переменной FLYGUARD_DATA.
_env = os.environ.get("FLYGUARD_DATA")
if _env:
DATA = Path(_env)
diff --git a/tools/build_brain_atlas.py b/tools/build_brain_atlas.py
new file mode 100644
index 0000000..3cad473
--- /dev/null
+++ b/tools/build_brain_atlas.py
@@ -0,0 +1,177 @@
+"""Атлас нейронов FlyWire: соматические координаты + привязка к стадиям FlyGuard.
+
+Берёт публичные выгрузки коннектома FAFB v783 (Codex) и сводит их в один
+компактный файл: положение каждого нейрона во фронтальной проекции и номер
+стадии конвейера, которой он соответствует. Дальше вид мозга рисуется как
+облако из 139 тысяч точек, подсвеченное живой активностью — без какой-либо
+симуляции: активность берётся из наших же стадий, а коннектом даёт только
+анатомию и принадлежность клеток.
+
+Привязка не выдумана: FlyGuard с самого начала собран из конкретных типов
+клеток, и все они есть в выгрузке поимённо — LC11 (127 нейронов), LPLC2 (210),
+HS (6), VS (16), T4 (6243), T5 (6002), клетки Кеньона (5177), MBON (96),
+APL (2), гигантское волокно DNp01 (2).
+
+Входные файлы (скачиваются сами, около 9 МБ):
+ coordinates.csv.gz root_id → положение сомы
+ classification.csv.gz super_class / class / sub_class / сторона
+ neurons.csv.gz нейропиль (group)
+ consolidated_cell_types.csv.gz имя типа клетки
+
+ python tools/build_brain_atlas.py
+
+Данные FlyWire распространяются под CC-BY 4.0 (Dorkenwald et al., Nature 2024;
+Schlegel et al., Nature 2024).
+"""
+from __future__ import annotations
+
+import argparse
+import csv
+import gzip
+import re
+import urllib.request
+from pathlib import Path
+
+import numpy as np
+
+import _bootstrap as B # noqa: F401
+
+BASE = "https://storage.googleapis.com/flywire-data/codex/data/fafb/783"
+FILES = ("coordinates", "classification", "neurons", "consolidated_cell_types")
+
+# Порядок важен: стадии проверяются сверху вниз, первая подошедшая выигрывает.
+# Именованный тип клетки сильнее нейропиля, нейропиль сильнее общего класса.
+STAGES = (
+ "retina", # 0 R1–R8, омматидиальная решётка
+ "lamina", # 1 L1/L2, ON/OFF и центр-окружение
+ "medulla", # 2 T4/T5, элементарные детекторы движения
+ "lptc", # 3 HS/VS, широкопольный поток → собственная скорость
+ "looming", # 4 LPLC2, надвигание
+ "lobula", # 5 LC11, мелкий объект
+ "mushroom", # 6 KC / APL / MBON, новизна
+ "central", # 7 PB/FB/EB/NO, накопление улик
+ "descending", # 8 нисходящие, решение
+ "other", # 9 остальной мозг — контекст, рисуется тускло
+)
+OTHER = len(STAGES) - 1
+
+CX_NEUROPILS = {"PB", "FB", "EB", "NO", "AB"}
+
+
+def _download(dest: Path) -> None:
+ dest.mkdir(parents=True, exist_ok=True)
+ for name in FILES:
+ p = dest / f"{name}.csv.gz"
+ if p.exists() and p.stat().st_size > 1000:
+ continue
+ print(f" качаю {name}.csv.gz …", flush=True)
+ urllib.request.urlretrieve(f"{BASE}/{name}.csv.gz", p)
+
+
+def _read(dest: Path, name: str):
+ with gzip.open(dest / f"{name}.csv.gz", "rt", encoding="utf-8") as f:
+ yield from csv.DictReader(f)
+
+
+def classify(cell_type: str, group: str, sub_class: str, super_class: str) -> int:
+ """Номер стадии FlyGuard для одного нейрона."""
+ t = cell_type or ""
+ if t in ("DNp01", "DNp02", "DNp11") or super_class == "descending":
+ return STAGES.index("descending")
+ if t == "LC11":
+ return STAGES.index("lobula")
+ if t == "LPLC2":
+ return STAGES.index("looming")
+ if re.fullmatch(r"HS[ENS]|VS\d+", t):
+ return STAGES.index("lptc")
+ if re.fullmatch(r"T[45][a-d]", t):
+ return STAGES.index("medulla")
+ if t in ("L1", "L2") or sub_class == "lamina_monopolar":
+ return STAGES.index("lamina")
+ if sub_class == "photo_receptor":
+ return STAGES.index("retina")
+ if t.startswith(("KC", "MBON")) or t == "APL":
+ return STAGES.index("mushroom")
+
+ parts = set(group.split(".")) if group else set()
+ if any(p.startswith("MB_") for p in parts):
+ return STAGES.index("mushroom")
+ if parts & CX_NEUROPILS:
+ return STAGES.index("central")
+ if "LA" in parts:
+ return STAGES.index("lamina")
+ # Остальная оптическая доля: медулла отвечает за движение, лобула — за форму.
+ if "LOP" in parts:
+ return STAGES.index("lptc")
+ if "LO" in parts:
+ return STAGES.index("lobula")
+ if "ME" in parts:
+ return STAGES.index("medulla")
+ return OTHER
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--src", default=str(B.DATA / "flywire"))
+ ap.add_argument("--out", default=str(B.ROOT / "ros2_ws" / "src" / "flyguard" /
+ "flyguard" / "data" / "brain_atlas.npz"))
+ ap.add_argument("--width", type=int, default=1180)
+ ap.add_argument("--height", type=int, default=620)
+ ap.add_argument("--margin", type=int, default=18)
+ args = ap.parse_args()
+
+ src = Path(args.src)
+ _download(src)
+
+ pos: dict[str, tuple[int, int, int]] = {}
+ for r in _read(src, "coordinates"):
+ rid = r["root_id"]
+ if rid in pos:
+ continue # берём первую точку на нейрон
+ x, y, z = (int(v) for v in r["position"].strip("[]").split())
+ pos[rid] = (x, y, z)
+
+ group = {r["root_id"]: r["group"] for r in _read(src, "neurons")}
+ ctype = {r["root_id"]: r["primary_type"]
+ for r in _read(src, "consolidated_cell_types")}
+
+ rid_list, stage_list, side_list = [], [], []
+ for r in _read(src, "classification"):
+ rid = r["root_id"]
+ if rid not in pos:
+ continue
+ rid_list.append(rid)
+ stage_list.append(classify(ctype.get(rid, ""), group.get(rid, ""),
+ r["sub_class"], r["super_class"]))
+ side_list.append({"left": 0, "right": 1}.get(r["side"], 2))
+
+ xyz = np.array([pos[r] for r in rid_list], np.float64)
+ stage = np.array(stage_list, np.int8)
+ side = np.array(side_list, np.int8)
+
+ # Фронтальная проекция: x — влево-вправо, y — вверх-вниз, z — вглубь.
+ # Масштаб общий по обеим осям, иначе мозг растянется.
+ W, H, m = args.width, args.height, args.margin
+ lo, hi = xyz[:, :2].min(0), xyz[:, :2].max(0)
+ k = min((W - 2 * m) / (hi[0] - lo[0]), (H - 2 * m) / (hi[1] - lo[1]))
+ px = np.rint((xyz[:, 0] - lo[0]) * k + (W - (hi[0] - lo[0]) * k) / 2)
+ py = np.rint((xyz[:, 1] - lo[1]) * k + (H - (hi[1] - lo[1]) * k) / 2)
+ z = xyz[:, 2]
+ depth = np.rint(255 * (z - z.min()) / max(np.ptp(z), 1.0))
+
+ np.savez_compressed(
+ args.out,
+ px=px.astype(np.int16), py=py.astype(np.int16),
+ stage=stage, side=side, depth=depth.astype(np.uint8),
+ stages=np.array(STAGES), shape=np.array([H, W], np.int32))
+
+ print(f"\nнейронов в атласе: {len(rid_list)} холст {W}×{H}")
+ for i, name in enumerate(STAGES):
+ n = int((stage == i).sum())
+ print(f" {i} {name:<11s} {n:6d}")
+ print(f"\nсохранено: {args.out} "
+ f"({Path(args.out).stat().st_size / 1024:.0f} КБ)")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/compare_centreline.py b/tools/compare_centreline.py
new file mode 100644
index 0000000..77381a9
--- /dev/null
+++ b/tools/compare_centreline.py
@@ -0,0 +1,124 @@
+"""Сравнение способов оценить осевую линию пути: свод тоннеля против полотна.
+
+Свод виден далеко, но в кривой уходит из поля зрения; полотно видно только
+вблизи, зато прямо задаёт положение пути. Скрипт печатает и рисует оба профиля,
+чтобы выбрать источник и границы применимости.
+
+ python tools/compare_centreline.py --bag data/for_hackathon/roundT_doubleT
+"""
+from __future__ import annotations
+
+import argparse
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt # noqa: E402
+import numpy as np # noqa: E402
+
+import _bootstrap as B # noqa: F401,E402
+from flyguard.bag import Bag # noqa: E402
+from flyguard.geometry import TrackFrame, fit_rail_plane # noqa: E402
+from flyguard.retina import ScanLayout # noqa: E402
+
+
+def slices(d_lo=8.0, d_hi=200.0, n=30):
+ e = np.geomspace(d_lo, d_hi, n + 1)
+ return list(zip(e[:-1], e[1:]))
+
+
+def bore_centre(tf):
+ out = []
+ for lo, hi in slices():
+ m = tf.valid & (tf.d >= lo) & (tf.d < hi) & (tf.h > 0.6) & (tf.h < 3.2)
+ n = int(m.sum())
+ if n < 60:
+ continue
+ u = tf.u[m]
+ a, b = np.percentile(u, (3, 97))
+ if b - a < 1.5:
+ continue
+ out.append((0.5 * (lo + hi), 0.5 * (a + b), n))
+ return np.array(out) if out else np.zeros((0, 3))
+
+
+def bed_centre(tf, track=0.0):
+ """Центр полотна: полоса вокруг уровня рельсов, узкая по ширине."""
+ out = []
+ ref = track
+ for lo, hi in slices(8.0, 140.0, 26):
+ m = (tf.valid & (tf.d >= lo) & (tf.d < hi)
+ & (tf.h > -0.20) & (tf.h < 0.45) & (np.abs(tf.u - ref) < 2.6))
+ n = int(m.sum())
+ if n < 40:
+ continue
+ u = tf.u[m]
+ a, b = np.percentile(u, (5, 95))
+ if not (0.8 < b - a < 5.0):
+ continue
+ c = 0.5 * (a + b)
+ out.append((0.5 * (lo + hi), c, n))
+ ref = c # следующий срез ищем рядом с найденным
+ return np.array(out) if out else np.zeros((0, 3))
+
+
+def fit_arc(d, u, w):
+ """u ≈ c1·d + c2·d² (без свободного члена: под поездом ось проходит через 0)."""
+ A = np.stack([d, d * d], axis=1) * w[:, None]
+ c, *_ = np.linalg.lstsq(A, u * w, rcond=None)
+ return c
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--bag", required=True)
+ ap.add_argument("--frames", type=int, default=8)
+ ap.add_argument("--stride", type=int, default=20)
+ args = ap.parse_args()
+
+ bag = Bag(args.bag)
+ layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
+ cols = layout.column_slice(40.0)
+ lay = layout.sub(cols)
+
+ fig, axes = plt.subplots(1, 2, figsize=(13, 5), dpi=110)
+ plane = None
+ radii_b, radii_d = [], []
+ for _, pc in bag.frames(stop=args.frames * args.stride, stride=args.stride):
+ img = layout.project(pc).crop(cols)
+ plane = fit_rail_plane(img, lay, prev=plane)
+ tf = TrackFrame(img, lay, plane)
+
+ bo = bore_centre(tf)
+ be = bed_centre(tf)
+ dd = np.linspace(0, 200, 100)
+ if bo.shape[0] >= 4:
+ u0 = bo[0, 1]
+ c = fit_arc(bo[:, 0], bo[:, 1] - u0, np.sqrt(bo[:, 2]))
+ axes[0].plot(bo[:, 0], bo[:, 1] - u0, ".", ms=4)
+ axes[0].plot(dd, c[0] * dd + c[1] * dd ** 2, lw=1, alpha=0.6)
+ radii_b.append(1 / (2 * c[1]) if c[1] else np.inf)
+ if be.shape[0] >= 4:
+ c = fit_arc(be[:, 0], be[:, 1], np.sqrt(be[:, 2]))
+ axes[1].plot(be[:, 0], be[:, 1], ".", ms=4)
+ axes[1].plot(dd, c[0] * dd + c[1] * dd ** 2, lw=1, alpha=0.6)
+ radii_d.append(1 / (2 * c[1]) if c[1] else np.inf)
+
+ for ax, t, r in ((axes[0], "центр свода тоннеля", radii_b),
+ (axes[1], "центр полотна пути", radii_d)):
+ ax.axhline(1.7, color="r", ls="--", lw=0.8); ax.axhline(-1.7, color="r", ls="--", lw=0.8)
+ med = np.median(r) if r else np.nan
+ spread = np.std(r) if len(r) > 1 else np.nan
+ ax.set_title(f"{t}\nR медиана {med:.0f} м, разброс {spread:.0f} м, оценок {len(r)}")
+ ax.set_xlabel("вперёд, м"); ax.set_ylabel("смещение оси, м")
+ ax.set_ylim(-14, 14); ax.set_xlim(0, 200)
+ print(f"{t}: R = " + ", ".join(f"{x:.0f}" for x in r))
+
+ B.FIGURES.mkdir(parents=True, exist_ok=True)
+ out = B.FIGURES / f"centreline_{bag.path.name}.png"
+ fig.suptitle(bag.path.name)
+ fig.tight_layout(); fig.savefig(out)
+ print("сохранено:", out)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/find_obstacle.py b/tools/find_obstacle.py
new file mode 100644
index 0000000..40713a0
--- /dev/null
+++ b/tools/find_obstacle.py
@@ -0,0 +1,55 @@
+"""Scan a bag for points inside the train gauge ahead of the sensor.
+
+Sensor frame: forward = -Y, lateral = X, up = Z.
+"""
+from __future__ import annotations
+
+import sys
+
+import numpy as np
+
+sys.path.insert(0, r"C:\Games\Study\AI_Lidar\tools")
+from probe_bag import frames # noqa: E402
+
+
+def main(db, n=None, half_width=1.5, z_lo=-0.2, z_hi=2.2, d_min=5.0, d_max=250.0):
+ print("frame ts floor_z n_gauge nearest clusters(d[m] x[m] z[m] npts)")
+ for k, (ts, m) in enumerate(frames(db, limit=n)):
+ p = m["points"]
+ x = p["x"].astype(np.float32)
+ y = p["y"].astype(np.float32)
+ z = p["z"].astype(np.float32)
+ v = ~((x == 0) & (y == 0) & (z == 0))
+ d = -y # forward distance
+
+ # estimate floor/rail level from near-field points under the sensor
+ near = v & (d > 4) & (d < 25) & (np.abs(x) < 2.0)
+ floor = np.percentile(z[near], 2) if near.sum() > 100 else np.nan
+
+ sel = v & (d > d_min) & (d < d_max) & (np.abs(x) < half_width) \
+ & (z > floor + z_lo) & (z < floor + z_hi)
+ idx = np.flatnonzero(sel)
+ info = ""
+ if idx.size:
+ ds = d[idx]
+ order = np.argsort(ds)
+ ds = ds[order]
+ idx = idx[order]
+ # crude 1-D clustering along the corridor
+ splits = np.flatnonzero(np.diff(ds) > 1.0) + 1
+ parts = np.split(np.arange(ds.size), splits)
+ chunks = []
+ for pr in parts:
+ if pr.size < 8:
+ continue
+ ii = idx[pr]
+ chunks.append((float(ds[pr].min()), float(np.median(x[ii])),
+ float(np.median(z[ii]) - floor), int(pr.size)))
+ chunks.sort(key=lambda c: -c[3])
+ info = " ".join(f"[{c[0]:6.1f} {c[1]:+5.2f} {c[2]:+5.2f} {c[3]:5d}]" for c in chunks[:4])
+ print(f"{k:5d} {ts/1e9:.2f} {floor:8.2f} {idx.size:8d} "
+ f"{(d[idx].min() if idx.size else float('nan')):8.1f} {info}")
+
+
+if __name__ == "__main__":
+ main(sys.argv[1], int(sys.argv[2]) if len(sys.argv) > 2 else None)
diff --git a/tools/probe_detect.py b/tools/probe_detect.py
new file mode 100644
index 0000000..142eb03
--- /dev/null
+++ b/tools/probe_detect.py
@@ -0,0 +1,90 @@
+"""Прогон геометрической части конвейера: сколько кандидатов и где.
+
+Это опорный эксперимент: на `doubleT_obstacle` обязан стабильно находиться
+реальный объект на ~55 м, на пустых бэгах — считается поток ложных кандидатов.
+
+ python tools/probe_detect.py --bag data/for_hackathon/doubleT_obstacle --frames 40 --verbose
+ python tools/probe_detect.py --all --frames 60
+"""
+from __future__ import annotations
+
+import argparse
+import time
+
+import numpy as np
+
+import _bootstrap as B # noqa: F401
+from flyguard import lamina
+from flyguard.bag import Bag, find_bags
+from flyguard.geometry import TrackFrame, fit_corridor, fit_rail_plane
+from flyguard.lobula import find_candidates
+from flyguard.retina import ScanLayout
+
+
+def run(bag_path, n_frames: int, stride: int, fov: float, verbose: bool,
+ **kw) -> dict:
+ bag = Bag(bag_path)
+ layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
+ cols = layout.column_slice(fov)
+ lay = layout.sub(cols)
+
+ plane = corridor = None
+ per_frame, times = [], []
+ hits = []
+ for k, (_, pc) in enumerate(bag.frames(stop=n_frames * stride, stride=stride)):
+ t0 = time.perf_counter()
+ img = layout.project(pc).crop(cols)
+ plane = fit_rail_plane(img, lay, prev=plane)
+ tf = TrackFrame(img, lay, plane)
+ corridor = fit_corridor(tf, prev=corridor)
+ lam = lamina.process(tf.r, tf.valid)
+ cands = find_candidates(tf, lam, corridor, **kw)
+ times.append(time.perf_counter() - t0)
+
+ per_frame.append(len(cands))
+ hits.extend(cands)
+ if verbose:
+ head = " ".join(
+ f"[{c.d:6.1f}м u{c.u:+5.2f} h{c.h:4.2f} {c.width:4.2f}×{c.height:4.2f}м "
+ f"n={c.n_rays:4d} gap={c.gap:6.1f} def={c.floor_deficit:6.1f}]"
+ for c in cands[:4])
+ print(f" кадр {k*stride:4d}: {len(cands):3d} канд. {head}")
+
+ n = np.array(per_frame)
+ ds = np.array([c.d for c in hits]) if hits else np.zeros(0)
+ res = dict(name=bag.path.name, frames=len(n),
+ mean=float(n.mean()) if n.size else 0.0,
+ median=float(np.median(n)) if n.size else 0.0,
+ p95=float(np.percentile(n, 95)) if n.size else 0.0,
+ zero=float(np.mean(n == 0)) if n.size else 0.0,
+ total=int(n.sum()),
+ t_ms=float(np.median(times) * 1e3),
+ far=int((ds > 60).sum()))
+ print(f"{res['name']:40s} кадров {res['frames']:4d} | кандидатов/кадр "
+ f"среднее {res['mean']:6.2f} медиана {res['median']:4.0f} p95 {res['p95']:5.0f} | "
+ f"пустых кадров {res['zero']:5.1%} | дальше 60 м {res['far']:5d} | "
+ f"{res['t_ms']:6.1f} мс/кадр")
+ return res
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--bag")
+ ap.add_argument("--all", action="store_true")
+ ap.add_argument("--frames", type=int, default=50)
+ ap.add_argument("--stride", type=int, default=2)
+ ap.add_argument("--fov", type=float, default=25.0)
+ ap.add_argument("--verbose", action="store_true")
+ ap.add_argument("--h-lo", type=float, default=0.28)
+ ap.add_argument("--min-rays", type=int, default=4)
+ ap.add_argument("--half-width", type=float, default=1.7)
+ args = ap.parse_args()
+
+ kw = dict(h_lo=args.h_lo, min_rays=args.min_rays, half_width=args.half_width)
+ bags = find_bags(B.DATA / "for_hackathon") if args.all else [args.bag]
+ for b in bags:
+ run(b, args.frames, args.stride, args.fov, args.verbose, **kw)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/render_brain.py b/tools/render_brain.py
new file mode 100644
index 0000000..b01331b
--- /dev/null
+++ b/tools/render_brain.py
@@ -0,0 +1,86 @@
+"""Отрисовка схемы мозга мухи по кадрам бэга — проверка и материал для видео.
+
+ python tools/render_brain.py --bag data/for_hackathon/doubleT_obstacle --frames 8
+ python tools/render_brain.py --bag ... --video docs/figures/brain.mp4 --limit 200
+"""
+from __future__ import annotations
+
+import argparse
+
+import numpy as np
+
+import _bootstrap as B # noqa: F401
+from flyguard.bag import Bag
+from flyguard.brain_view import BrainView
+from flyguard.mushroom_body import MushroomBody
+from flyguard.pipeline import FlyGuard, Params
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--bag", required=True)
+ ap.add_argument("--memory")
+ ap.add_argument("--limit", type=int, default=120)
+ ap.add_argument("--frames", type=int, default=6, help="сколько кадров сохранить в PNG")
+ ap.add_argument("--video", default=None)
+ ap.add_argument("--style", default="hybrid",
+ choices=("scheme", "cloud", "hybrid"),
+ help="схема | облако нейронов | панели + облако")
+ args = ap.parse_args()
+
+ if args.style == "scheme":
+ view = BrainView()
+ elif args.style == "cloud":
+ from flyguard.brain_atlas import NeuronCloud
+ view = NeuronCloud()
+ else:
+ from flyguard.brain_hybrid import BrainHybrid
+ view = BrainHybrid()
+ if not view.enabled:
+ raise SystemExit("нужен OpenCV, а для облака — ещё атлас нейронов "
+ "(python tools/build_brain_atlas.py)")
+ import cv2
+
+ memory = MushroomBody.load(args.memory) if args.memory else None
+ fg = FlyGuard(Params(enable_looming=True), memory=memory)
+ bag = Bag(args.bag)
+
+ B.FIGURES.mkdir(parents=True, exist_ok=True)
+ writer = None
+ saved = 0
+ processed = 0
+ # кадры калибровки решётки не доходят до отрисовки, поэтому шаг считается
+ # от реально обрабатываемых, иначе при малом --limit не сохранится ничего
+ usable = max(args.limit - Params().calib_frames, 1)
+ every = max(1, usable // max(args.frames, 1))
+
+ for k, (_, pc) in enumerate(bag.frames(stop=args.limit)):
+ res = fg.process(pc, keep_debug=True)
+ if res is None:
+ continue
+ processed += 1
+ img = view.render(res)
+ if img is None:
+ continue
+ if args.video:
+ if writer is None:
+ fourcc = cv2.VideoWriter_fourcc(*"mp4v")
+ writer = cv2.VideoWriter(args.video, fourcc, 10.0,
+ (img.shape[1], img.shape[0]))
+ writer.write(img)
+ if processed % every == 0 and saved < args.frames:
+ out = B.FIGURES / f"brain_{args.style}_{bag.path.name}_{k:04d}.png"
+ cv2.imwrite(str(out), img)
+ saved += 1
+ print(f" кадр {k:4d}: {out.name} "
+ f"{'ПРЕПЯТСТВИЕ %.0f м' % res.decision.distance if res.decision.detected else 'путь свободен'}"
+ f" {res.total_ms:.1f} мс")
+
+ if writer is not None:
+ writer.release()
+ print("видео:", args.video)
+ print(f"обработано кадров: {processed}, сохранено картинок: {saved}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/rviz_layout.py b/tools/rviz_layout.py
new file mode 100644
index 0000000..3e195f7
--- /dev/null
+++ b/tools/rviz_layout.py
@@ -0,0 +1,103 @@
+"""Раскладка панелей RViz: строка «QMainWindow State» для конфига без Qt.
+
+Где и какой ширины стоят панели, RViz хранит в конфиге сериализацией Qt —
+шестнадцатеричной строкой `QMainWindow::saveState()`. Руками её не набрать, а
+Qt с привязками для Python в образе нет (и из WSL нет выхода к apt). Формат
+при этом простой и стабильный, он собран здесь по исходникам Qt 5.15
+(`QDockAreaLayout::saveState`, `QDockAreaLayoutInfo::saveState`): маркер
+версии, маркер доков, по каждой зоне — размер и список панелей с именем,
+флагами, положением и размером. Панель находится по имени объекта, а у RViz
+это имя панели или дисплея («Displays», «Мозг мухи»).
+
+ python tools/rviz_layout.py
+
+печатает строки для обоих конфигов: `config/flyguard.rviz` (панель Displays
+слева) и `config/flyguard_brain.rviz` (плюс схема мозга справа во всю высоту).
+"""
+from __future__ import annotations
+
+import argparse
+import struct
+
+QWIDGETSIZE_MAX = 16777215
+LEFT, RIGHT = 0, 1 # QInternal::DockPosition
+VERTICAL = 2 # Qt::Vertical — панели в зоне идут столбиком
+CORNERS = (4, 4, 8, 8) # углы окна: верхняя, верхняя, нижняя, нижняя зона
+
+
+def _i32(v: int) -> bytes:
+ return struct.pack(">i", v)
+
+
+def _qstring(s: str) -> bytes:
+ b = s.encode("utf-16-be")
+ return struct.pack(">I", len(b)) + b
+
+
+def state(areas, central: int, height: int) -> str:
+ """areas — [(зона, ширина, [имя панели, ...])], central — ширина трёхмерного вида."""
+ out = _i32(0xff) + _i32(0) + b"\xfd" + _i32(len(areas))
+ for pos, width, docks in areas:
+ out += _i32(pos) + _i32(width) + _i32(height)
+ out += b"\xfc" + bytes([VERTICAL]) + _i32(len(docks))
+ step = height // len(docks)
+ for k, name in enumerate(docks):
+ out += b"\xfb" + _qstring(name) + b"\x01" # видима, не плавает
+ out += _i32(k * step) + _i32(step) + _i32(100) + _i32(QWIDGETSIZE_MAX)
+ out += _i32(central) + _i32(height)
+ for c in CORNERS:
+ out += _i32(c)
+ return out.hex()
+
+
+def parse(hexs: str):
+ """Обратный разбор — проверка, что строка собрана без хвостов."""
+ b = bytes.fromhex(hexs)
+ p = 0
+
+ def rd(fmt):
+ nonlocal p
+ v = struct.unpack_from(fmt, b, p)
+ p += struct.calcsize(fmt)
+ return v
+
+ assert rd(">ii") == (0xff, 0) and rd(">B") == (0xfd,)
+ n, = rd(">i")
+ areas = []
+ for _ in range(n):
+ pos, w, h = rd(">iii")
+ marker, _, cnt = rd(">BBi")
+ assert marker == 0xfc
+ docks = []
+ for _ in range(cnt):
+ assert rd(">B") == (0xfb,)
+ ln, = rd(">I")
+ docks.append(b[p:p + ln].decode("utf-16-be"))
+ p += ln
+ rd(">Biiii")
+ areas.append((pos, w, h, docks))
+ central = rd(">ii")
+ rd(">iiii")
+ assert p == len(b), "строка не разобрана до конца"
+ return areas, central
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__,
+ formatter_class=argparse.RawDescriptionHelpFormatter)
+ ap.add_argument("--width", type=int, default=1880, help="ширина окна, как Width в конфиге")
+ ap.add_argument("--height", type=int, default=900, help="высота зоны панелей")
+ ap.add_argument("--left", type=int, default=300, help="ширина панели Displays")
+ ap.add_argument("--brain", type=int, default=820, help="ширина схемы мозга")
+ a = ap.parse_args()
+ plain = state([(LEFT, a.left, ["Displays"])], a.width - a.left - 4, a.height)
+ left = a.left - 20
+ brain = state([(LEFT, left, ["Displays"]), (RIGHT, a.brain, ["Мозг мухи"])],
+ a.width - left - a.brain - 8, a.height)
+ for name, s in (("flyguard.rviz", plain), ("flyguard_brain.rviz", brain)):
+ parse(s)
+ print(f"{name}:\n QMainWindow State: {s}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/stream_new_data.py b/tools/stream_new_data.py
new file mode 100644
index 0000000..7a74013
--- /dev/null
+++ b/tools/stream_new_data.py
@@ -0,0 +1,109 @@
+"""Потоковый прогон большого бэга прямо из tar-архива.
+
+`new_data` — это 90 ГБ в 221 шарде, и распаковывать его целиком некуда. Скрипт
+достаёт по одному шарду во временный каталог, прогоняет конвейер, забирает
+дескрипторы кандидатов и удаляет шард. Состояние конвейера (поза, коридор,
+собственное движение, треки) переносится между шардами, поэтому запись
+обрабатывается как одна непрерывная поездка.
+
+ python tools/stream_new_data.py --shards 60 --out data/cache/new_data_candidates.npz
+"""
+from __future__ import annotations
+
+import argparse
+import re
+import shutil
+import tarfile
+import tempfile
+import time
+from pathlib import Path
+
+import numpy as np
+
+import _bootstrap as B # noqa: F401
+from flyguard.bag import Bag
+from flyguard.mushroom_body import describe
+from flyguard.pipeline import FlyGuard, Params
+
+SHARD_RE = re.compile(r"_(\d+)\.db3$")
+
+
+def shard_members(tar_path: Path):
+ """Список (номер, TarInfo) шардов, отсортированный по номеру."""
+ out = []
+ with tarfile.open(tar_path, "r:") as t:
+ for m in t:
+ mm = SHARD_RE.search(m.name)
+ if mm and m.isfile():
+ out.append((int(mm.group(1)), m.name, m.size))
+ out.sort()
+ return out
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--tar", default=r"C:\Games\Study\AI_Lidar\датасет\new_data")
+ ap.add_argument("--out", default=str(B.CACHE / "new_data_candidates.npz"))
+ ap.add_argument("--shards", type=int, default=0, help="0 — все")
+ ap.add_argument("--stride", type=int, default=1, help="прореживание кадров")
+ ap.add_argument("--workdir", default=None)
+ args = ap.parse_args()
+
+ tar_path = Path(args.tar)
+ B.CACHE.mkdir(parents=True, exist_ok=True)
+ members = shard_members(tar_path)
+ if args.shards:
+ members = members[:args.shards]
+ print(f"шардов к обработке: {len(members)} из архива {tar_path.name}")
+
+ work = Path(args.workdir) if args.workdir else Path(tempfile.mkdtemp(prefix="flyguard_"))
+ work.mkdir(parents=True, exist_ok=True)
+
+ fg = FlyGuard(Params(), memory=None)
+ rows, dists, frames_total = [], [], 0
+ t_start = time.time()
+
+ with tarfile.open(tar_path, "r:") as tar:
+ for k, (num, name, size) in enumerate(members):
+ shard_dir = work / "bag"
+ if shard_dir.exists():
+ shutil.rmtree(shard_dir)
+ shard_dir.mkdir(parents=True)
+ target = shard_dir / Path(name).name
+
+ src = tar.extractfile(name)
+ with open(target, "wb") as dst:
+ shutil.copyfileobj(src, dst, length=1 << 22)
+
+ n_here = 0
+ try:
+ bag = Bag(shard_dir)
+ for _, pc in bag.frames(stride=args.stride):
+ res = fg.process(pc)
+ n_here += 1
+ if res is None:
+ continue
+ for c in res.candidates:
+ rows.append(describe(c))
+ dists.append(c.d)
+ except Exception as exc: # шард может быть обрезан
+ print(f" шард {num}: пропущен ({exc})")
+ frames_total += n_here
+ target.unlink(missing_ok=True)
+
+ if k % 5 == 0 or k == len(members) - 1:
+ el = time.time() - t_start
+ print(f" шард {k+1}/{len(members)} (#{num}) кадров {n_here:4d} | "
+ f"всего кадров {frames_total:6d} кандидатов {len(rows):7d} | "
+ f"{el/60:5.1f} мин", flush=True)
+
+ shutil.rmtree(work, ignore_errors=True)
+ X = np.stack(rows).astype(np.float32) if rows else np.zeros((0, 21), np.float32)
+ np.savez_compressed(args.out, X=X, d=np.asarray(dists, np.float32),
+ frames=frames_total)
+ print(f"готово: {frames_total} кадров, {X.shape[0]} кандидатов → {args.out}")
+ print(f"время: {(time.time()-t_start)/60:.1f} мин")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/viz_frame.py b/tools/viz_frame.py
new file mode 100644
index 0000000..d87e6c4
--- /dev/null
+++ b/tools/viz_frame.py
@@ -0,0 +1,124 @@
+"""Диагностические картинки одного кадра: дальностный образ, срезы, вид сверху.
+
+ python tools/viz_frame.py --bag data/for_hackathon/doubleT_obstacle --frame 40
+"""
+from __future__ import annotations
+
+import argparse
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt # noqa: E402
+import numpy as np # noqa: E402
+
+import _bootstrap as B # noqa: F401,E402
+from flyguard.bag import Bag # noqa: E402
+from flyguard.retina import ScanLayout # noqa: E402
+
+
+def load(bag_path: str, frame: int, n_calib: int = 12):
+ bag = Bag(bag_path)
+ clouds = [pc for _, pc in bag.frames(start=2, stop=2 + n_calib)]
+ layout = ScanLayout.calibrate(clouds)
+ _, pc = next(bag.frames(start=frame, stop=frame + 1))
+ return bag, layout, layout.project(pc)
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--bag", required=True)
+ ap.add_argument("--frame", type=int, default=40)
+ ap.add_argument("--fov", type=float, default=30.0, help="полусектор по азимуту, °")
+ ap.add_argument("--out", default=None)
+ args = ap.parse_args()
+
+ bag, layout, img = load(args.bag, args.frame)
+ cols = layout.column_slice(args.fov)
+ lay = layout.sub(cols)
+ sub = img.crop(cols)
+
+ xyz = sub.xyz(lay)
+ x, y, z = xyz[..., 0], xyz[..., 1], xyz[..., 2]
+ d = -y # вперёд
+ v = sub.valid
+
+ # уровень пола по ближней зоне под сенсором
+ near = v & (d > 4) & (d < 25) & (np.abs(x) < 2.0)
+ z_rail = float(np.percentile(z[near], 2)) if near.sum() > 200 else np.nan
+ h = z - z_rail
+
+ fig = plt.figure(figsize=(19, 12), dpi=110)
+ fig.suptitle(f"{bag.path.name} кадр {args.frame} "
+ f"пол z={z_rail:.2f} м валидных {v.mean():.0%}", fontsize=13)
+
+ # 1. дальностный образ
+ ax = fig.add_subplot(3, 2, 1)
+ disp = np.where(v, sub.r_near, np.nan)
+ im = ax.imshow(disp, aspect="auto", cmap="turbo", vmin=0, vmax=120,
+ extent=[lay.az_grid_deg[0], lay.az_grid_deg[-1],
+ lay.el_deg[-1], lay.el_deg[0]])
+ ax.set_title("дальностный образ (выпрямленный), м")
+ ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
+ fig.colorbar(im, ax=ax, fraction=0.03)
+
+ # 2. интенсивность
+ ax = fig.add_subplot(3, 2, 2)
+ im = ax.imshow(np.where(v, sub.inten, np.nan), aspect="auto", cmap="magma",
+ vmin=0, vmax=60,
+ extent=[lay.az_grid_deg[0], lay.az_grid_deg[-1],
+ lay.el_deg[-1], lay.el_deg[0]])
+ ax.set_title("интенсивность")
+ ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
+ fig.colorbar(im, ax=ax, fraction=0.03)
+
+ # 3. вид сверху
+ ax = fig.add_subplot(3, 2, 3)
+ m = v & (d > 0) & (d < 200)
+ ax.scatter(d[m], x[m], s=0.05, c=h[m], cmap="viridis", vmin=-1, vmax=4, linewidths=0)
+ for s in (-1.5, 1.5):
+ ax.axhline(s, color="r", lw=0.8, ls="--")
+ ax.set_xlim(0, 200); ax.set_ylim(-8, 8)
+ ax.set_title("вид сверху (цвет — высота над головкой рельса)")
+ ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("поперёк, м")
+
+ # 4. вид сбоку
+ ax = fig.add_subplot(3, 2, 4)
+ ax.scatter(d[m], h[m], s=0.05, c=np.abs(x[m]), cmap="plasma", vmin=0, vmax=4, linewidths=0)
+ ax.axhline(0, color="k", lw=0.8)
+ ax.axhline(2.1, color="r", lw=0.8, ls="--")
+ ax.set_xlim(0, 200); ax.set_ylim(-3, 6)
+ ax.set_title("вид сбоку (цвет — |поперёк|)")
+ ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("высота над рельсом, м")
+
+ # 5. поперечные сечения
+ ax = fig.add_subplot(3, 2, 5)
+ for lo, hi, c in [(5, 15, "tab:blue"), (30, 45, "tab:orange"),
+ (60, 80, "tab:green"), (100, 130, "tab:red")]:
+ s = v & (d >= lo) & (d < hi)
+ ax.scatter(x[s], h[s], s=0.3, alpha=0.4, color=c, label=f"{lo}–{hi} м", linewidths=0)
+ ax.add_patch(plt.Rectangle((-1.5, 0), 3.0, 2.1, fill=False, ec="r", lw=1.5))
+ ax.set_xlim(-6, 6); ax.set_ylim(-2, 6); ax.set_aspect("equal")
+ ax.legend(fontsize=8, markerscale=8)
+ ax.set_title("поперечные сечения (красное — габарит 3.0 × 2.1 м)")
+ ax.set_xlabel("поперёк, м"); ax.set_ylabel("высота, м")
+
+ # 6. профиль дальности по лучам вдоль пути
+ ax = fig.add_subplot(3, 2, 6)
+ bins = np.arange(0, 205, 2.0)
+ for lo, hi, name in [(-1.0, 1.0, "|az|<1°"), (-5, 5, "|az|<5°"), (-20, 20, "|az|<20°")]:
+ sel = (lay.az_grid_deg >= lo) & (lay.az_grid_deg <= hi)
+ dd = d[:, sel][v[:, sel]]
+ ax.hist(dd, bins=bins, histtype="step", label=name, log=True)
+ ax.set_title("распределение дальностей по секторам")
+ ax.set_xlabel("расстояние вперёд, м"); ax.set_ylabel("число лучей")
+ ax.legend(fontsize=8)
+
+ out = args.out or (B.FIGURES / f"frame_{bag.path.name}_{args.frame:04d}.png")
+ B.FIGURES.mkdir(parents=True, exist_ok=True)
+ fig.tight_layout(rect=(0, 0, 1, 0.97))
+ fig.savefig(out)
+ print("сохранено:", out)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/viz_object.py b/tools/viz_object.py
new file mode 100644
index 0000000..e278ed6
--- /dev/null
+++ b/tools/viz_object.py
@@ -0,0 +1,100 @@
+"""Крупный план участка дальностного образа вокруг заданной точки пространства.
+
+Нужен, чтобы понять, как реальный объект выглядит в ретинотопической решётке:
+размер в лучах, контраст по дальности, окклюзионная тень за ним.
+
+ python tools/viz_object.py --bag data/for_hackathon/doubleT_obstacle \
+ --frame 40 --dist 55 --lat -0.8
+"""
+from __future__ import annotations
+
+import argparse
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt # noqa: E402
+import numpy as np # noqa: E402
+
+import _bootstrap as B # noqa: F401,E402
+from flyguard.bag import Bag # noqa: E402
+from flyguard.retina import ScanLayout # noqa: E402
+
+
+def main() -> None:
+ ap = argparse.ArgumentParser(description=__doc__)
+ ap.add_argument("--bag", required=True)
+ ap.add_argument("--frame", type=int, default=40)
+ ap.add_argument("--dist", type=float, required=True, help="расстояние вперёд, м")
+ ap.add_argument("--lat", type=float, default=0.0, help="смещение поперёк, м")
+ ap.add_argument("--pad-az", type=float, default=4.0, help="окно по азимуту, °")
+ ap.add_argument("--pad-el", type=float, default=4.0, help="окно по элевации, °")
+ args = ap.parse_args()
+
+ bag = Bag(args.bag)
+ layout = ScanLayout.calibrate([pc for _, pc in bag.frames(start=2, stop=14)])
+ _, pc = next(bag.frames(start=args.frame, stop=args.frame + 1))
+ img = layout.project(pc)
+
+ az_c = np.degrees(np.arctan2(args.lat, args.dist))
+ el_c = 0.0
+ ja = np.flatnonzero(np.abs(layout.az_grid_deg - az_c) <= args.pad_az)
+ ie = np.flatnonzero(np.abs(layout.el_deg - el_c) <= args.pad_el)
+ js, je = slice(ja[0], ja[-1] + 1), slice(ie[0], ie[-1] + 1)
+
+ r = np.where(img.valid, img.r_near, np.nan)[je, js]
+ it = np.where(img.valid, img.inten, np.nan)[je, js]
+ gap = np.where(img.valid, img.r_far - img.r_near, np.nan)[je, js]
+ az = layout.az_grid_deg[js]
+ el = layout.el_deg[je]
+ ext = [az[0], az[-1], el[-1], el[0]]
+
+ print(f"окно: азимут {az.min():.2f}…{az.max():.2f}° ({az.size} стлб), "
+ f"элевация {el.min():.2f}…{el.max():.2f}° ({el.size} колец)")
+ band = r[np.isfinite(r)]
+ print(f"дальности в окне: p5={np.percentile(band, 5):.1f} p50={np.percentile(band, 50):.1f} "
+ f"p95={np.percentile(band, 95):.1f} max={band.max():.1f}")
+ near = np.isfinite(r) & (np.abs(r - args.dist) < 5)
+ print(f"лучей на дистанции {args.dist}±5 м: {near.sum()}, "
+ f"доля окна {near.mean():.1%}")
+ print(f"пустых лучей в окне: {np.mean(~np.isfinite(r)):.1%}")
+
+ fig, axes = plt.subplots(2, 2, figsize=(15, 9), dpi=110)
+ fig.suptitle(f"{bag.path.name} кадр {args.frame}: окрестность точки "
+ f"({args.dist:.0f} м вперёд, {args.lat:+.1f} м поперёк)")
+
+ ax = axes[0, 0]
+ im = ax.imshow(r, extent=ext, aspect="auto", cmap="turbo", interpolation="nearest")
+ ax.set_title("дальность, м"); fig.colorbar(im, ax=ax, fraction=0.04)
+
+ ax = axes[0, 1]
+ lo, hi = np.nanpercentile(r, [2, 98])
+ im = ax.imshow(r, extent=ext, aspect="auto", cmap="turbo", vmin=lo, vmax=hi,
+ interpolation="nearest")
+ ax.set_title(f"дальность, растянутая шкала {lo:.0f}…{hi:.0f} м")
+ fig.colorbar(im, ax=ax, fraction=0.04)
+
+ ax = axes[1, 0]
+ im = ax.imshow(it, extent=ext, aspect="auto", cmap="magma", interpolation="nearest")
+ ax.set_title("интенсивность"); fig.colorbar(im, ax=ax, fraction=0.04)
+
+ ax = axes[1, 1]
+ im = ax.imshow(gap, extent=ext, aspect="auto", cmap="cividis", vmin=0, vmax=10,
+ interpolation="nearest")
+ ax.set_title("разнос эх (дальнее − ближнее), м")
+ fig.colorbar(im, ax=ax, fraction=0.04)
+
+ for ax in axes.ravel():
+ ax.set_xlabel("азимут, °"); ax.set_ylabel("элевация, °")
+ ax.invert_xaxis()
+ ax.axvline(az_c, color="w", lw=0.6, ls=":")
+ ax.axhline(el_c, color="w", lw=0.6, ls=":")
+
+ B.FIGURES.mkdir(parents=True, exist_ok=True)
+ out = B.FIGURES / f"object_{bag.path.name}_{args.frame:04d}_{int(args.dist)}m.png"
+ fig.tight_layout(rect=(0, 0, 1, 0.95))
+ fig.savefig(out)
+ print("сохранено:", out)
+
+
+if __name__ == "__main__":
+ main()