Merge pull request 'main' (#2) from Zhirik1337/Brainrot_Muxa:main into main

Reviewed-on: Dan4ick/Brainrot_Muxa#2

tools/flyguard_ros2_node.py

@@ -0,0 +1,216 @@
+"""
+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()