""" 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 ROOT_DIR = Path(__file__).resolve().parent.parent if str(ROOT_DIR) not in sys.path: sys.path.insert(0, str(ROOT_DIR)) import rclpy from rclpy.node import Node from geometry_msgs.msg import Point from sensor_msgs.msg import PointCloud2 from std_msgs.msg import String from vision_msgs.msg import BoundingBox3D, Detection3D, Detection3DArray, ObjectHypothesisWithPose from visualization_msgs.msg import Marker, MarkerArray 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 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', '/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') 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') 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) 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.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): 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 if res is None: # идёт калибровка решётки по первым кадрам return self.n_frames += 1 frame_id = self.frame_id or msg.header.frame_id out = export_frame(res.decision, self.fg.plane, self.fg.corridor, stamp=res.stamp) 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) 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) 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 = arr.header bbox = BoundingBox3D() 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 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) 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 _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 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): rclpy.init(args=args) node = FlyGuardNode() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.try_shutdown() if __name__ == '__main__': main()