From 657cd5406f4e4d16de49dbdc8e394e6c8b7a68b0 Mon Sep 17 00:00:00 2001 From: Hitoshi-Hub Date: Tue, 22 Sep 2026 21:24:02 +0300 Subject: [PATCH] ros2+Dockerfile --- Dockerfile | 22 ++++ Dockerfile.ros2 | 28 +++++ flyguard/flyguard_ros2_node.py | 216 +++++++++++++++++++++++++++++++++ 3 files changed, 266 insertions(+) create mode 100644 Dockerfile create mode 100644 Dockerfile.ros2 create mode 100644 flyguard/flyguard_ros2_node.py diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..c6f9410 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,22 @@ +# FlyGuard processing core and command-line tools (CPU only). +FROM python:3.11-slim + +ENV PYTHONDONTWRITEBYTECODE=1 \ + PYTHONUNBUFFERED=1 \ + PIP_NO_CACHE_DIR=1 + +WORKDIR /app + +# Runtime dependencies for the core are supplied as wheels for this platform. +COPY requirements.txt ./ +RUN pip install --no-cache-dir -r requirements.txt + +COPY flyguard ./flyguard +COPY tools ./tools +COPY tests ./tests +COPY artifacts ./artifacts + +# The default verifies the containerised core. Override CMD for a tool, e.g.: +# docker run --rm -v /path/to/data:/data flyguard \ +# python tools/run_pipeline.py --bag /data/my_bag --memory artifacts/mushroom_body.npz +CMD ["pytest", "tests", "-q"] diff --git a/Dockerfile.ros2 b/Dockerfile.ros2 new file mode 100644 index 0000000..ee84b20 --- /dev/null +++ b/Dockerfile.ros2 @@ -0,0 +1,28 @@ +# ROS 2 Humble integration image for FlyGuard. +FROM ros:humble-ros-base-jammy + +ENV DEBIAN_FRONTEND=noninteractive \ + PYTHONDONTWRITEBYTECODE=1 \ + PYTHONUNBUFFERED=1 \ + PIP_NO_CACHE_DIR=1 + +WORKDIR /app + +# ROS message packages used directly by flyguard_ros2_node.py. +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + python3-pip \ + ros-humble-sensor-msgs-py \ + ros-humble-vision-msgs \ + ros-humble-visualization-msgs \ + && rm -rf /var/lib/apt/lists/* + +COPY requirements.txt ./ +RUN python3 -m pip install --no-cache-dir -r requirements.txt + +COPY flyguard ./flyguard +COPY artifacts ./artifacts + +# Parameters and ROS remappings are passed to `docker run` after `--ros-args`. +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["python3", "flyguard/flyguard_ros2_node.py"] diff --git a/flyguard/flyguard_ros2_node.py b/flyguard/flyguard_ros2_node.py new file mode 100644 index 0000000..6f68801 --- /dev/null +++ b/flyguard/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