38 lines
1.8 KiB
Python
38 lines
1.8 KiB
Python
"""Преобразование sensor_msgs/PointCloud2 из rclpy во внутреннее представление.
|
|
|
|
Внутри ноды сообщение уже разобрано транспортом, поэтому CDR-парсер не нужен:
|
|
достаточно посмотреть на поля и построить структурированный numpy-массив
|
|
поверх готового буфера, без копирования.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
|
|
from .cdr import PointCloud2, point_dtype
|
|
|
|
|
|
def from_ros(msg) -> PointCloud2:
|
|
"""sensor_msgs.msg.PointCloud2 → flyguard.cdr.PointCloud2 (без копирования)."""
|
|
fields = [(f.name, f.offset, f.datatype, f.count) for f in msg.fields]
|
|
fields.sort(key=lambda f: f[1])
|
|
dt = point_dtype(fields, msg.point_step)
|
|
buf = msg.data if isinstance(msg.data, (bytes, bytearray, memoryview)) else \
|
|
np.asarray(msg.data, np.uint8).tobytes()
|
|
n = msg.height * msg.width
|
|
pts = np.frombuffer(buf, dtype=dt, count=n)
|
|
stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
|
|
return PointCloud2(stamp=stamp, frame_id=msg.header.frame_id,
|
|
height=msg.height, width=msg.width,
|
|
point_step=msg.point_step, is_dense=msg.is_dense, points=pts)
|
|
|
|
|
|
def has_required_fields(msg) -> tuple[bool, str]:
|
|
"""Проверить, что в облаке есть всё необходимое конвейеру."""
|
|
names = {f.name for f in msg.fields}
|
|
need = {"x", "y", "z", "intensity"}
|
|
missing = need - names
|
|
if missing:
|
|
return False, f"в облаке нет полей: {', '.join(sorted(missing))}"
|
|
if msg.height * msg.width == 0:
|
|
return False, "пустое облако"
|
|
return True, ""
|