forked from Dan4ick/Lidar_Muxa
765 lines
41 KiB
Python
765 lines
41 KiB
Python
"""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),
|
||
# канал малых целей (LC11): кандидат из 2–3 лучей в пустоте габарита, 0 — выключить
|
||
("small_rays", 2),
|
||
("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"]),
|
||
small_rays=int(self.par["small_rays"]),
|
||
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._started_at = time.monotonic()
|
||
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()
|
||
# Издателей не видно вовсе: запись ещё не запущена — или контейнер без
|
||
# --network host, и обнаружение DDS до хоста не доходит. Без подсказки
|
||
# второе выглядит как узел, который просто ждёт.
|
||
waited = time.monotonic() - self._started_at
|
||
if waited >= 15.0:
|
||
self.get_logger().warn(
|
||
f"облака точек нет уже {waited:.0f} с: издателя не видно ни на одном "
|
||
f"топике. Если запись проигрывается на хосте, контейнер нужен с "
|
||
f"--network host --ipc host", throttle_duration_sec=30.0)
|
||
return
|
||
self._match_publisher_qos()
|
||
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 _match_publisher_qos(self) -> None:
|
||
"""Издатель шлёт облако «best effort» — подписаться так же.
|
||
|
||
Надёжный подписчик с таким издателем по правилам DDS несовместим: кадры
|
||
не придут вовсе, и узел будет выглядеть как пустой тоннель. Наши записи
|
||
и синтетика организаторов записаны с надёжной доставкой, а
|
||
`ros2 bag play` публикует с записанным QoS, — но драйвер лидара обычно
|
||
публикует облако как «best effort», и запись с живого поезда может
|
||
прийти такой. Подписчик «best effort» принимает от издателей обоих
|
||
видов, однако крупный кадр надёжнее доставляется надёжно (конфиг,
|
||
`best_effort`), поэтому переключение — только по факту несовместимости.
|
||
"""
|
||
if self._qos.reliability != QoSReliabilityPolicy.RELIABLE:
|
||
return
|
||
loose = sorted({t for t in self._topics
|
||
for info in self.get_publishers_info_by_topic(t)
|
||
if info.qos_profile.reliability == QoSReliabilityPolicy.BEST_EFFORT})
|
||
if not loose:
|
||
return
|
||
self._qos = QoSProfile(history=self._qos.history, depth=self._qos.depth,
|
||
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||
durability=self._qos.durability)
|
||
for sub in self.subs:
|
||
self.destroy_subscription(sub)
|
||
self.subs = [self.create_subscription(PointCloud2, t, self._on_cloud, self._qos,
|
||
raw=self.raw)
|
||
for t in self._topics]
|
||
self._pub_seen_at = None
|
||
self.get_logger().warn(
|
||
f"{', '.join(loose)}: издатель шлёт облако «best effort», а узел ждал "
|
||
f"надёжной доставки — такие не соединяются вовсе. Переподписался "
|
||
f"«best effort» (явно: best_effort:=true)")
|
||
|
||
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()
|