216 lines
No EOL
8 KiB
Python
216 lines
No EOL
8 KiB
Python
"""
|
||
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() |