"""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), # висящее посреди габарита: пол вероятности считывания, 0 — выключить ("hover_floor", 0.5), ("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"]), hover_floor=float(self.par["hover_floor"]), 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._told_forward = False # сказано ли, что облако пришлось поворачивать 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.calib_progress}/{self.fg.p.calib_frames}", throttle_duration_sec=2.0) return if self.fg.forward_deg and not self._told_forward: self._told_forward = True self.get_logger().warn( f"облако повёрнуто: «вперёд» у него на азимуте {self.fg.forward_deg:+.0f}° " f"от −Y, как в выданных записях, — узел поворачивает кадры сам") 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()