diff --git a/tools/flyguard_ros2_node.py b/tools/flyguard_ros2_node.py new file mode 100644 index 0000000..6f68801 --- /dev/null +++ b/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() \ No newline at end of file