""" ROS 2 узел для биоинспирированного пайплайна FlyGuard (ЛЦТ-2026 Кейс 05). Связывает входные данные 3D-лидара с ядром FlyGuard и публикует детекции/маркеры. """ import sys 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)) import rclpy from rclpy.node import Node # Импорт сообщений ROS 2 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 import sensor_msgs_py.point_cloud2 as pc2 # Импорт ядра FlyGuard try: from flyguard.pipeline import FlyGuard, Params from flyguard.mushroom_body import MushroomBody except ImportError as e: FlyGuard = None Params = None MushroomBody = 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('memory_path', '') self.declare_parameter('fov_deg', 30.0) 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 # --- Инициализация ядра 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 # --- Подписчики и Публикаторы --- 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_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}") 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: return # 2. Вызываем ядро FlyGuard if self.fg is None: return res = self.fg.process(pc_data) if res is None: return d = res.decision tracks = getattr(self.fg.cx, 'tracks', []) # 3. Определяем уровень угрозы if d.detected: threat_level = "EMERGENCY" if getattr(d, 'emergency', False) else "WARNING" else: threat_level = "CLEAR" # 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: det = Detection3D() det.header = det_array.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)) det.bbox = bbox det_array.detections.append(det) self.pub_boxes.publish(det_array) 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) 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 main(args=None): rclpy.init(args=args) node = FlyGuardNode() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()