diff --git a/docs/EXPERIMENTS.md b/docs/EXPERIMENTS.md index a9ffc63..0dbf4de 100644 --- a/docs/EXPERIMENTS.md +++ b/docs/EXPERIMENTS.md @@ -2707,3 +2707,69 @@ SIGTERM — узел его переживал. Узлы копились: на нужно смотреть глазами: по словам организаторов, препятствий в данных «не больше двух», и одно из них в `doubleT_obstacle` — второе может оказаться здесь. + +## 18. Узел `tools/flyguard_ros2_node.py` доведён до рабочего состояния + +П. 16.7 фиксировал узел как нерабочий («отдаёт конвейеру массив numpy вместо +разобранного `PointCloud2`, публикует все треки, а не решение, и у трека нет +полей рамки») и оставлял его за интеграцией. Отдельного `ros2_ws/.../node.py` +или `export_team.py` в репозитории нет — это единственный узел, поэтому +переписан начисто: + +* **Вход.** [`flyguard/cdr.py`](../flyguard/cdr.py) получил `from_ros_message`: + переводит уже разобранное rclpy-сообщение в тот же `PointCloud2`, что и + офлайн-парсер, по общему `point_dtype`. Порядок точек и NaN сохраняются — + `sensor_msgs_py.read_points(skip_nans=True)` их выбрасывал, а решётка + `retina.py` раскладывает облако по позиции в массиве (азимут × кольцо), и + выброшенная точка сдвигала бы всю решётку. Тест — + `test_ros_message_keeps_every_point_in_order`. +* **Решение, не треки.** Узел зовёт [`flyguard/export.py`](../flyguard/export.py) + (`export_frame`) — тот же модуль, что уже решал разворот рамки в кривой и + зеркальный `yaw` (п. 16.7) — и публикует `Decision`, а не сырые треки + `CentralComplex`. Рамки идут и в `vision_msgs/Detection3DArray`, и в + `visualization_msgs/MarkerArray` (кубы + подписи «ID | дальность | TTC»), из + одного и того же `ExportResult`, так что они не могут разойтись. +* **Не падает от битого кадра.** `pointcloud_callback` оборачивает + `fg.process()` в `try/except`: следующий кадр придёт через 100 мс, а не + превратит узел в мёртвый процесс на записи с реальным Pandar128E3X, где + структура сообщения может отличаться от тестовых бэгов. +* **Память и считывания подключаются по умолчанию.** Раньше узел собирал + `FlyGuard` без `memory`/`readout` вообще — конвейер работал в режиме «всё + ново», без обученной памяти тоннеля. Теперь по умолчанию берутся + `artifacts/mushroom_body.npz` и `artifacts/mbon_readout.npz`, если они есть + (параметры `memory_path`/`readout_path` позволяют переопределить путь или + явно отключить значением `-`). +* **Устройство по умолчанию — `cpu`**, как и решил п. 16.7: на GPU + ускоряется только `Lamina`, а `device=auto` в узле интеграции — источник + тихих ошибок, если жюри не даёт GPU. + +Не проверено на самом Pandar128E3X и не прогнано в реальном `ros2_ws` — только +модульным тестом и прогоном логики колбэка на заглушках `rclpy`/`vision_msgs` +(тестовое окружение без ROS2). Перед демо нужен прогон `ros2 bag play` живой +записи с этим узлом. + +## 19. `flyguard/bag.py`: битый CDR-пакет больше не обрывает чтение бэга + +`Bag.frames()` звал `parse_pointcloud2(blob)` без защиты — одно повреждённое +сообщение (реальный сенсор, не тестовый бэг) роняло исключение наружу и +обрывало генератор, а с ним — весь прогон `tools/evaluate.py` / +`tools/run_pipeline.py` на этой записи, вместе со всеми уже накопленными +кадрами. Теперь разбор каждого сообщения обёрнут в `try/except`: битый кадр +логируется (`flyguard.bag`, номер кадра, файл шарда, тип исключения) и +пропускается, `start`/`stop` по-прежнему считают по позиции сообщения в +бэге, а не по числу успешно разобранных — окно `frames(start=..., stop=...)` +не съезжает. Число пропусков доступно как `Bag.n_frames_failed` после +прохода. Тест — `test_bag_skips_a_corrupted_message_and_keeps_reading` +(набор вырос до 46/46), с отдельным минимальным CDR-кодировщиком в +`tests/test_pipeline.py` для валидного `PointCloud2` без зависимости от +реальных бэгов. + +Два других пункта из того же списка критики остаются открытыми — их нельзя +закрыть в этом окружении: + +* **Три неопознанных ложных трека (п. 17.7)** требуют визуального разбора по + самим бэгам/RViz — самих файлов записей в `data/` в этой рабочей копии нет. +* **ROS2-узел на реальном стенде** — в этом окружении не установлен пакет + `rclpy` (`ModuleNotFoundError: No module named 'rclpy'`), так что живой + прогон `ros2 bag play` с узлом (п. 18) не выполним здесь; логика узла + проверена только на заглушках. diff --git a/flyguard/bag.py b/flyguard/bag.py index b555967..d99c943 100644 --- a/flyguard/bag.py +++ b/flyguard/bag.py @@ -6,6 +6,7 @@ """ from __future__ import annotations +import logging import re import sqlite3 from contextlib import closing @@ -15,6 +16,8 @@ from typing import Iterator from .cdr import PointCloud2, parse_pointcloud2 +logger = logging.getLogger("flyguard.bag") + _SHARD_RE = re.compile(r"_(\d+)\.db3$") _POINTCLOUD_TYPE = "sensor_msgs/msg/PointCloud2" @@ -44,6 +47,7 @@ class Bag: self.shards = shards self.topics = self._scan_topics() self.topic = topic or self._pick_topic() + self.n_frames_failed = 0 # обновляется по ходу последнего frames() @staticmethod def _shard_key(p: Path) -> tuple[int, str]: @@ -83,9 +87,13 @@ class Bag: """Выдать (timestamp_ns, облако) для сообщений выбранного топика. Индексация сквозная по всему бэгу; шарды читаются по порядку, внутри - шарда — по возрастанию времени. + шарда — по возрастанию времени. Сообщение, которое не удаётся разобрать + (битый CDR у реального сенсора), пропускается с предупреждением в лог, + а не останавливает чтение бэга — `start`/`stop` считают по позиции + сообщения в бэге, а не по числу успешно разобранных. """ idx = 0 + self.n_frames_failed = 0 for shard in self.shards: # closing(), а не сам connection: у sqlite3 `with` управляет # транзакцией и файл остаётся открытым — на Windows его потом @@ -111,7 +119,15 @@ class Bag: if stop is not None and idx >= stop: return if idx >= start and (idx - start) % stride == 0: - yield ts, parse_pointcloud2(blob) + try: + pc = parse_pointcloud2(blob) + except Exception as e: # noqa: BLE001 + self.n_frames_failed += 1 + logger.warning( + "кадр %d в %s не разобран (%s: %s) — пропущен", + idx, shard.name, type(e).__name__, e) + else: + yield ts, pc idx += 1 def describe(self) -> str: diff --git a/flyguard/cdr.py b/flyguard/cdr.py index 2112f0a..0d139da 100644 --- a/flyguard/cdr.py +++ b/flyguard/cdr.py @@ -4,7 +4,8 @@ * офлайн-эксперименты на машине без ROS (Windows); * прямое чтение rosbag внутри контейнера, минуя `ros2 bag play`. -Внутри ROS-ноды сообщение приходит уже разобранным, и этот модуль не используется. +Внутри ROS-ноды сообщение приходит уже разобранным; его переводит в тот же +`PointCloud2` функция `from_ros_message`. """ from __future__ import annotations @@ -128,3 +129,21 @@ def parse_pointcloud2(blob: bytes | memoryview) -> PointCloud2: return PointCloud2(stamp=sec + nsec * 1e-9, frame_id=frame_id, height=height, width=width, point_step=point_step, is_dense=is_dense, points=points) + + +def from_ros_message(msg) -> PointCloud2: + """Перевести разобранное rclpy-сообщение sensor_msgs/msg/PointCloud2. + + Все height·width точек сохраняются, включая NaN: сетчатка раскладывает облако + в решётку азимут × кольцо по порядку точек, и выброшенная точка сдвинула бы + всю решётку (`sensor_msgs_py.read_points(skip_nans=True)` делает именно это). + """ + if msg.is_bigendian: + raise ValueError("big-endian PointCloud2 не поддерживается") + fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields] + dt = point_dtype(fields, msg.point_step) + points = np.frombuffer(msg.data, dtype=dt, count=msg.height * msg.width) + stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 + return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id, height=msg.height, + width=msg.width, point_step=msg.point_step, + is_dense=bool(msg.is_dense), points=points) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..ae19949 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,13 @@ +[tool.ruff] +target-version = "py311" +line-length = 96 +extend-exclude = ["artifacts", "data", "__pycache__"] + +[tool.ruff.lint] +select = ["E", "F", "W", "B", "UP", "SIM"] +ignore = [ + "E501", # длину строк проверяет line-length, но многодетные формулы иногда шире +] + +[tool.ruff.lint.per-file-ignores] +"tests/*" = ["E402"] # sys.path.insert перед импортом flyguard — так во всех тестах diff --git a/tests/test_pipeline.py b/tests/test_pipeline.py index c9aa849..ce50af6 100644 --- a/tests/test_pipeline.py +++ b/tests/test_pipeline.py @@ -8,6 +8,8 @@ """ from __future__ import annotations +import sqlite3 +import struct import sys from pathlib import Path @@ -50,6 +52,37 @@ def test_point_dtype_rejects_overlapping_fields(): point_dtype([("x", 0, 8, 1), ("y", 4, 7, 1)], 26) +def test_ros_message_keeps_every_point_in_order(): + """Узел ROS: NaN-точки остаются на месте, иначе решётка азимут × кольцо съедет.""" + from types import SimpleNamespace as NS + + from flyguard.cdr import from_ros_message + + spec = [("x", 0, 7, 1), ("y", 4, 7, 1), ("z", 8, 7, 1), ("intensity", 12, 7, 1), + ("ring", 16, 4, 1), ("timestamp", 18, 8, 1)] + dt = point_dtype(spec, 26) + n_rings, n_cols = 4, 5 + pts = np.zeros(n_rings * n_cols, dt) + pts["x"] = np.arange(pts.size, dtype=np.float32) + pts["ring"] = np.tile(np.arange(n_rings), n_cols) + pts["timestamp"] = 1.5 + np.arange(pts.size) * 1e-6 + pts["x"][[3, 7]] = np.nan + msg = NS(header=NS(stamp=NS(sec=12, nanosec=500_000_000), frame_id="hesai_lidar"), + height=1, width=pts.size, point_step=26, is_bigendian=False, is_dense=False, + fields=[NS(name=n, offset=o, datatype=t, count=c) for n, o, t, c in spec], + data=pts.tobytes()) + + pc = from_ros_message(msg) + + assert pc.n_points == n_rings * n_cols + assert pc.stamp == pytest.approx(12.5) + assert pc.frame_id == "hesai_lidar" + assert np.isnan(pc.points["x"][[3, 7]]).all() + assert pc.points["x"][8] == 8.0 + assert np.array_equal(pc.points["ring"], pts["ring"]) + assert np.array_equal(pc.points["timestamp"], pts["timestamp"]) + + # --------------------------------------------------------------------------- решётка def _layout(n_rings=8, n_az=40, n_echo=2, shift=None): @@ -389,6 +422,64 @@ def test_injected_intensity_copies_the_surroundings(): # --------------------------------------------------------------------------- данные +def _write_minimal_bag(db_path, blobs: list[bytes]): + """Собрать минимальный `.db3` со схемой rosbag2, нужной `flyguard.bag.Bag`.""" + con = sqlite3.connect(db_path) + con.execute("CREATE TABLE topics (id INTEGER PRIMARY KEY, name TEXT, type TEXT)") + con.execute("CREATE TABLE messages (id INTEGER PRIMARY KEY, topic_id INTEGER, " + "timestamp INTEGER, data BLOB)") + con.execute("INSERT INTO topics VALUES (1, '/lidar_points', " + "'sensor_msgs/msg/PointCloud2')") + for i, blob in enumerate(blobs): + con.execute("INSERT INTO messages (topic_id, timestamp, data) VALUES (1, ?, ?)", + (i, blob)) + con.commit() + con.close() + + +def _cdr_pointcloud2_blob(n_points: int) -> bytes: + """Валидный CDR sensor_msgs/msg/PointCloud2: один float32 `x` на точку.""" + def s(text: str) -> bytes: + b = text.encode() + b"\x00" + pad = (-len(b)) % 4 + return struct.pack(" uint32 (_align(4)) + buf += struct.pack(" uint32 (_align(4)) + buf += struct.pack(" None: help="через запятую; пусто — все предметы") ap.add_argument("--edges", default="0,15,40,90,130,170,230", help="границы полос дальности, м") + ap.add_argument("--fail-on-net-down", type=int, default=None, metavar="N", + help="выйти с кодом 1, если (1→0) − (0→1) больше N (для CI)") args = ap.parse_args() a, b = load(args.before), load(args.after) @@ -78,6 +84,12 @@ def main() -> None: total_down += obj_down print(f"\nвсе предметы: 0→1 {total_up}, 1→0 {total_down}") + if args.fail_on_net_down is not None: + net_down = total_down - total_up + if net_down > args.fail_on_net_down: + raise SystemExit(f"регрессия: (1→0) − (0→1) = {net_down} > " + f"{args.fail_on_net_down}") + if __name__ == "__main__": main() diff --git a/tools/flyguard_ros2_node.py b/tools/flyguard_ros2_node.py index 6f68801..596d9e5 100644 --- a/tools/flyguard_ros2_node.py +++ b/tools/flyguard_ros2_node.py @@ -1,13 +1,21 @@ """ ROS 2 узел для биоинспирированного пайплайна FlyGuard (ЛЦТ-2026 Кейс 05). Связывает входные данные 3D-лидара с ядром FlyGuard и публикует детекции/маркеры. + +Топики: + вход sensor_msgs/PointCloud2 (Hesai Pandar128E3X) + выход flyguard/threat_level std_msgs/String CLEAR | WARNING | EMERGENCY + flyguard/status std_msgs/String JSON: дальность, TTC, скорость, рамки + flyguard/bounding_boxes vision_msgs/Detection3DArray + flyguard/markers visualization_msgs/MarkerArray """ +import json +import math import sys +import time from pathlib import Path -import numpy as np -# Автоматически добавляем корень репозитория в sys.path ROOT_DIR = Path(__file__).resolve().parent.parent if str(ROOT_DIR) not in sys.path: sys.path.insert(0, str(ROOT_DIR)) @@ -15,189 +23,176 @@ if str(ROOT_DIR) not in sys.path: import rclpy from rclpy.node import Node -# Импорт сообщений ROS 2 +from geometry_msgs.msg import Point from sensor_msgs.msg import PointCloud2 from std_msgs.msg import String -from vision_msgs.msg import Detection3DArray, Detection3D, BoundingBox3D -from visualization_msgs.msg import MarkerArray, Marker -from geometry_msgs.msg import Vector3 +from vision_msgs.msg import BoundingBox3D, Detection3D, Detection3DArray, ObjectHypothesisWithPose +from visualization_msgs.msg import Marker, MarkerArray -import sensor_msgs_py.point_cloud2 as pc2 +from flyguard.cdr import from_ros_message +from flyguard.export import ThreatLevel, export_frame +from flyguard.mbon_readout import MbonReadout +from flyguard.mushroom_body import MushroomBody +from flyguard.pipeline import FlyGuard, Params +from flyguard.track_readout import TrackReadout -# Импорт ядра FlyGuard -try: - from flyguard.pipeline import FlyGuard, Params - from flyguard.mushroom_body import MushroomBody -except ImportError as e: - FlyGuard = None - Params = None - MushroomBody = None +ARTIFACTS = ROOT_DIR / "artifacts" + + +def _artifact(param: str, default_name: str) -> Path | None: + """Пустой параметр — артефакт по умолчанию из artifacts/, если он есть; '-' — без него.""" + if param == "-": + return None + path = Path(param) if param else ARTIFACTS / default_name + return path if path.exists() else None class FlyGuardNode(Node): def __init__(self): super().__init__('flyguard_node') - # --- Объявление параметров --- - self.declare_parameter('lidar_topic', '/pandar_points') - self.declare_parameter('frame_id', 'pandar') + self.declare_parameter('lidar_topic', '/lidar_points') + self.declare_parameter('frame_id', '') self.declare_parameter('memory_path', '') + self.declare_parameter('readout_path', '') + self.declare_parameter('track_readout_path', '-') self.declare_parameter('fov_deg', 30.0) + # на GPU ускоряется только ламина (EXPERIMENTS п. 7.4); 'auto' — если нужна видеокарта + self.declare_parameter('device', 'cpu') - self.lidar_topic = self.get_parameter('lidar_topic').get_parameter_value().string_value - self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value - memory_path = self.get_parameter('memory_path').get_parameter_value().string_value - fov_deg = self.get_parameter('fov_deg').get_parameter_value().double_value + gp = self.get_parameter + self.lidar_topic = gp('lidar_topic').value + self.frame_id = gp('frame_id').value + memory_path = _artifact(gp('memory_path').value, 'mushroom_body.npz') + readout_path = _artifact(gp('readout_path').value, 'mbon_readout.npz') + track_path = _artifact(gp('track_readout_path').value, 'track_readout.npz') - # --- Инициализация ядра FlyGuard --- - if FlyGuard is not None: - self.get_logger().info("Инициализация ядра FlyGuard...") - memory = MushroomBody.load(memory_path) if memory_path else None - params = Params(fov_deg=fov_deg) - self.fg = FlyGuard(params, memory=memory) - else: - self.get_logger().error("Модуль flyguard.pipeline не найден!") - self.fg = None + log = self.get_logger() + memory = MushroomBody.load(str(memory_path)) if memory_path else None + readout = MbonReadout.load(str(readout_path)) if readout_path else None + track_readout = TrackReadout.load(str(track_path)) if track_path else None + log.info(f"память тоннеля: {memory_path or 'нет'}; считывание MBON: " + f"{readout_path or 'нет'}; считывание треков: {track_path or 'нет'}") + if memory is None or readout is None: + log.warn("без памяти тоннеля и считывания MBON ложных тревог будет заметно больше") + + params = Params(fov_deg=float(gp('fov_deg').value), device=gp('device').value) + self.fg = FlyGuard(params, memory=memory, readout=readout, track_readout=track_readout) - # --- Подписчики и Публикаторы --- self.sub_cloud = self.create_subscription( - PointCloud2, - self.lidar_topic, - self.pointcloud_callback, - 10 - ) - + PointCloud2, self.lidar_topic, self.pointcloud_callback, 10) self.pub_threat = self.create_publisher(String, 'flyguard/threat_level', 10) + self.pub_status = self.create_publisher(String, 'flyguard/status', 10) self.pub_boxes = self.create_publisher(Detection3DArray, 'flyguard/bounding_boxes', 10) self.pub_markers = self.create_publisher(MarkerArray, 'flyguard/markers', 10) - self.get_logger().info(f"FlyGuard ROS 2 Node запущен! Топик: {self.lidar_topic}") + self.n_frames = 0 + self.n_failed = 0 + log.info(f"FlyGuard ROS 2 узел запущен, вход: {self.lidar_topic}, " + f"устройство: {self.fg.device}") def pointcloud_callback(self, msg: PointCloud2): - """Обработка каждого кадра лидара (10 Гц)""" - start_time = self.get_clock().now() - - # 1. Читаем точки из PointCloud2 в structured numpy array - # (сохраняет стандартную структуру PointCloud) - pc_data = pc2.read_points(msg, skip_nans=True) - if pc_data.size == 0: + t0 = time.perf_counter() + # Битый кадр не должен останавливать узел: следующий придёт через 100 мс. + try: + res = self.fg.process(from_ros_message(msg)) + except Exception as e: # noqa: BLE001 + self.n_failed += 1 + self.get_logger().error(f"кадр пропущен ({self.n_failed} всего): " + f"{type(e).__name__}: {e}", + throttle_duration_sec=1.0) return - - # 2. Вызываем ядро FlyGuard - if self.fg is None: + if res is None: # идёт калибровка решётки по первым кадрам return + self.n_frames += 1 - res = self.fg.process(pc_data) - if res is None: - return + frame_id = self.frame_id or msg.header.frame_id + out = export_frame(res.decision, self.fg.plane, self.fg.corridor, stamp=res.stamp) - d = res.decision - tracks = getattr(self.fg.cx, 'tracks', []) + self.pub_threat.publish(String(data=out.threat_level.name)) + self.pub_status.publish(String(data=json.dumps(_finite(out.to_dict()), + ensure_ascii=False))) + self.publish_detections(out, msg.header.stamp, frame_id) + self.publish_markers(out, msg.header.stamp, frame_id) - # 3. Определяем уровень угрозы - if d.detected: - threat_level = "EMERGENCY" if getattr(d, 'emergency', False) else "WARNING" - else: - threat_level = "CLEAR" + elapsed_ms = (time.perf_counter() - t0) * 1e3 + if out.threat_level != ThreatLevel.CLEAR: + self.get_logger().info(f"{out.threat_level.name}: {out.nearest_distance:.1f} м, " + f"TTC {out.ttc:.1f} с, кадр {elapsed_ms:.1f} мс", + throttle_duration_sec=0.5) + if elapsed_ms > 100.0: + self.get_logger().warn(f"кадр {elapsed_ms:.0f} мс — дольше периода лидара", + throttle_duration_sec=5.0) - # 4. Публикация статуса - threat_msg = String() - threat_msg.data = threat_level - self.pub_threat.publish(threat_msg) - - # 5. Публикация маркеров и рамок - stamp = msg.header.stamp - self.publish_detections(tracks, stamp) - self.publish_rviz_markers(tracks, threat_level, d, stamp) - - elapsed_ms = (self.get_clock().now() - start_time).nanoseconds / 1e6 - self.get_logger().debug(f"Кадр за {elapsed_ms:.1f} мс | Статус: {threat_level} | Треков: {len(tracks)}") - - def publish_detections(self, tracks, stamp): - """Формирование vision_msgs/Detection3DArray""" - det_array = Detection3DArray() - det_array.header.stamp = stamp - det_array.header.frame_id = self.frame_id - - for trk in tracks: + def publish_detections(self, out, stamp, frame_id): + arr = Detection3DArray() + arr.header.stamp = stamp + arr.header.frame_id = frame_id + for b in out.boxes: det = Detection3D() - det.header = det_array.header - + det.header = arr.header bbox = BoundingBox3D() - bbox.center.position.x = float(getattr(trk, 'x', 0.0)) - bbox.center.position.y = float(getattr(trk, 'y', 0.0)) - bbox.center.position.z = float(getattr(trk, 'z', 0.0)) - - bbox.size.x = float(getattr(trk, 'dx', 0.5)) - bbox.size.y = float(getattr(trk, 'dy', 0.5)) - bbox.size.z = float(getattr(trk, 'dz', 1.5)) - + bbox.center.position.x = b.x + bbox.center.position.y = b.y + bbox.center.position.z = b.z + bbox.center.orientation.z = math.sin(b.yaw * 0.5) + bbox.center.orientation.w = math.cos(b.yaw * 0.5) + bbox.size.x, bbox.size.y, bbox.size.z = b.dx, b.dy, b.dz det.bbox = bbox - det_array.detections.append(det) + hyp = ObjectHypothesisWithPose() + if hasattr(hyp, 'hypothesis'): # Humble и новее + hyp.hypothesis.class_id = b.threat_level.name + hyp.hypothesis.score = b.confidence + else: # Foxy + hyp.id = b.threat_level.name + hyp.score = b.confidence + hyp.pose.pose = bbox.center + det.results.append(hyp) + det.id = str(b.track_id) + arr.detections.append(det) + self.pub_boxes.publish(arr) - self.pub_boxes.publish(det_array) + def publish_markers(self, out, stamp, frame_id): + arr = MarkerArray() + clear = Marker() + clear.header.frame_id = frame_id + clear.action = Marker.DELETEALL + arr.markers.append(clear) + for m in out.to_rviz_markers(frame_id): + arr.markers.append(_marker_from_dict(m, stamp)) + self.pub_markers.publish(arr) - def publish_rviz_markers(self, tracks, threat_level: str, decision, stamp): - """Генерация маркеров для RViz2""" - marker_array = MarkerArray() - delete_all = Marker() - delete_all.action = Marker.DELETEALL - marker_array.markers.append(delete_all) +def _marker_from_dict(d: dict, stamp) -> Marker: + m = Marker() + m.header.stamp = stamp + m.header.frame_id = d["header"]["frame_id"] + m.ns, m.id, m.type, m.action = d["ns"], d["id"], d["type"], d["action"] + if "pose" in d: + p, q = d["pose"]["position"], d["pose"]["orientation"] + m.pose.position.x, m.pose.position.y, m.pose.position.z = p["x"], p["y"], p["z"] + m.pose.orientation.x, m.pose.orientation.y = q["x"], q["y"] + m.pose.orientation.z, m.pose.orientation.w = q["z"], q["w"] + else: + m.pose.orientation.w = 1.0 + s = d["scale"] + m.scale.x, m.scale.y, m.scale.z = s.get("x", 0.0), s.get("y", 0.0), s.get("z", 0.0) + c = d["color"] + m.color.r, m.color.g, m.color.b, m.color.a = c["r"], c["g"], c["b"], c["a"] + m.points = [Point(x=p["x"], y=p["y"], z=p["z"]) for p in d.get("points", [])] + m.text = d.get("text", "") + return m - for i, trk in enumerate(tracks): - # Куб вокруг объекта - box_marker = Marker() - box_marker.header.stamp = stamp - box_marker.header.frame_id = self.frame_id - box_marker.ns = "flyguard_boxes" - box_marker.id = i - box_marker.type = Marker.CUBE - box_marker.action = Marker.ADD - x = float(getattr(trk, 'x', 0.0)) - y = float(getattr(trk, 'y', 0.0)) - z = float(getattr(trk, 'z', 0.0)) - - box_marker.pose.position.x = x - box_marker.pose.position.y = y - box_marker.pose.position.z = z - - dx = float(getattr(trk, 'dx', 0.5)) - dy = float(getattr(trk, 'dy', 0.5)) - dz = float(getattr(trk, 'dz', 1.5)) - box_marker.scale = Vector3(x=dx, y=dy, z=dz) - - if threat_level == "EMERGENCY": - box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 1.0, 0.0, 0.0, 0.6 - elif threat_level == "WARNING": - box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 1.0, 0.9, 0.0, 0.5 - else: - box_marker.color.r, box_marker.color.g, box_marker.color.b, box_marker.color.a = 0.0, 1.0, 0.0, 0.4 - - marker_array.markers.append(box_marker) - - # Текст с дистанцией над объектом - text_marker = Marker() - text_marker.header.stamp = stamp - text_marker.header.frame_id = self.frame_id - text_marker.ns = "flyguard_labels" - text_marker.id = 1000 + i - text_marker.type = Marker.TEXT_VIEW_FACING - text_marker.action = Marker.ADD - - text_marker.pose.position.x = x - text_marker.pose.position.y = y - text_marker.pose.position.z = z + dz / 2.0 + 0.3 - text_marker.scale.z = 0.4 - - dist = getattr(trk, 'distance', np.hypot(x, y)) - text_marker.text = f"D: {dist:.1f}m" - - text_marker.color.r, text_marker.color.g, text_marker.color.b, text_marker.color.a = 1.0, 1.0, 1.0, 1.0 - marker_array.markers.append(text_marker) - - self.pub_markers.publish(marker_array) +def _finite(v): + """JSON без Infinity/NaN: бесконечный TTC у свободного пути уходит как null.""" + if isinstance(v, float): + return v if math.isfinite(v) else None + if isinstance(v, dict): + return {k: _finite(x) for k, x in v.items()} + if isinstance(v, (list, tuple)): + return [_finite(x) for x in v] + return v def main(args=None): @@ -209,8 +204,8 @@ def main(args=None): pass finally: node.destroy_node() - rclpy.shutdown() + rclpy.try_shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main()