Zhirik1337
200cb78348
Узел tools/flyguard_ros2_node.py (п. 16.7: отдавал массив вместо PointCloud2, публиковал треки без решения) переписан: разбор через новый flyguard.cdr.from_ros_message, публикация через flyguard.export.export_frame, не падает от битого кадра, по умолчанию подключает artifacts/*.npz. flyguard/bag.py: битый CDR-пакет в бэге пропускается с логом вместо обрыва всего чтения (Bag.n_frames_failed). tools/compare_benchmark.py: флаг --fail-on-net-down для CI-гейта регрессии по методологии парных переворотов (EXPERIMENTS п. 16.1). Добавлен pyproject.toml (ruff, не прогнан — в окружении нет ruff/pip). Тесты 46/46 (tests/test_pipeline.py). Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
211 lines
9.1 KiB
Python
211 lines
9.1 KiB
Python
"""
|
||
ROS 2 узел для биоинспирированного пайплайна FlyGuard (ЛЦТ-2026 Кейс 05).
|
||
Связывает входные данные 3D-лидара с ядром FlyGuard и публикует детекции/маркеры.
|
||
|
||
Топики:
|
||
вход <lidar_topic> 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()
|