Архитектура решения

Цепочка, которую просит ТЗ: 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 мс без изменения результата (проверено побитовым сравнением).

12 KiB Raw Blame History Unescape Escape