372 lines
12 KiB
Python
372 lines
12 KiB
Python
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
|
||
|
||
Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
|
||
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||
distance_mm = raw * distance_unit_mm, then:
|
||
|
||
x = d_m * cos(alt) * cos(az)
|
||
y = d_m * cos(alt) * sin(az)
|
||
z = d_m * sin(alt)
|
||
|
||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
||
-16°…+16° fan, horizontal channel offsets default to 0.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass
|
||
|
||
import numpy as np
|
||
|
||
from capture_format_v2 import CaptureFile
|
||
|
||
PACKET_LENGTH = 1248
|
||
DATA_START = 42
|
||
BLOCKS = 12
|
||
BLOCK_LENGTH = 100
|
||
CHANNELS = 32
|
||
MIN_FRAME_POINTS_DEFAULT = 100
|
||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||
|
||
|
||
def ticks_to_unix_ns(ticks: int) -> int:
|
||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||
|
||
|
||
def default_vertical_deg() -> np.ndarray:
|
||
return -16.0 + np.arange(CHANNELS, dtype=np.float64) * (32.0 / (CHANNELS - 1))
|
||
|
||
|
||
def default_horizontal_deg() -> np.ndarray:
|
||
return np.zeros(CHANNELS, dtype=np.float64)
|
||
|
||
|
||
def read_u16_be(packet: bytes, index: int) -> int:
|
||
return (packet[index] << 8) | packet[index + 1]
|
||
|
||
|
||
def device_timestamp_ms(packet: bytes) -> int:
|
||
seconds = int.from_bytes(packet[20:26], "big")
|
||
microseconds = int.from_bytes(packet[26:30], "big")
|
||
return seconds * 1000 + microseconds // 1000
|
||
|
||
|
||
def distance_unit_mm(packet: bytes, *, auto: bool = True, fallback: float = 2.5) -> float:
|
||
if not auto:
|
||
return float(fallback)
|
||
return 2.5 if packet[17] == 1 else 0.5
|
||
|
||
|
||
def normalize_azimuth_deg(angle: float) -> float:
|
||
while angle > 180.0:
|
||
angle -= 360.0
|
||
while angle < -180.0:
|
||
angle += 360.0
|
||
return angle
|
||
|
||
|
||
@dataclass
|
||
class LidarFrameExport:
|
||
t_start_s: float
|
||
t_end_s: float
|
||
points_xyz: np.ndarray # (N, 3) metres
|
||
|
||
|
||
@dataclass
|
||
class LidarFramePolarExport:
|
||
"""One H32 frame in the calibration ``points_raw`` polar contract.
|
||
|
||
Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``.
|
||
Azimuth already includes the H32 channel horizontal offset and sign flip so
|
||
``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points.
|
||
"""
|
||
|
||
t_start_s: float
|
||
t_end_s: float
|
||
points_raw: np.ndarray # (N, 5) float32
|
||
host_receive_utc_ns: int
|
||
|
||
|
||
def decode_packet_points(
|
||
packet: bytes,
|
||
vertical_deg: np.ndarray,
|
||
horizontal_deg: np.ndarray,
|
||
*,
|
||
min_range_m: float = 0.3,
|
||
max_range_m: float = 120.0,
|
||
) -> tuple[list[float], np.ndarray]:
|
||
"""Decode one MSOP packet into block azimuths and concatenated XYZ points."""
|
||
|
||
if len(packet) != PACKET_LENGTH:
|
||
return [], np.zeros((0, 3), dtype=np.float64)
|
||
unit = distance_unit_mm(packet)
|
||
az_list: list[float] = []
|
||
chunks: list[np.ndarray] = []
|
||
idx = DATA_START
|
||
for _block in range(BLOCKS):
|
||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||
break
|
||
az = read_u16_be(packet, idx + 2) * 0.01
|
||
az_list.append(az)
|
||
pts = _block_points(
|
||
packet,
|
||
idx,
|
||
az,
|
||
unit,
|
||
vertical_deg,
|
||
horizontal_deg,
|
||
min_range_m=min_range_m,
|
||
max_range_m=max_range_m,
|
||
)
|
||
if pts.shape[0]:
|
||
chunks.append(pts)
|
||
idx += BLOCK_LENGTH
|
||
if not chunks:
|
||
return az_list, np.zeros((0, 3), dtype=np.float64)
|
||
return az_list, np.vstack(chunks)
|
||
|
||
|
||
def _block_points(
|
||
packet: bytes,
|
||
block_offset: int,
|
||
az_deg: float,
|
||
unit_mm: float,
|
||
vertical_deg: np.ndarray,
|
||
horizontal_deg: np.ndarray,
|
||
*,
|
||
min_range_m: float,
|
||
max_range_m: float,
|
||
) -> np.ndarray:
|
||
xs: list[float] = []
|
||
ys: list[float] = []
|
||
zs: list[float] = []
|
||
idx = block_offset + 4 # after FF EE + azimuth
|
||
for ch in range(CHANNELS):
|
||
raw = read_u16_be(packet, idx)
|
||
idx += 3
|
||
if raw == 0:
|
||
continue
|
||
d_m = (raw * unit_mm) * 0.001
|
||
if d_m < min_range_m or d_m > max_range_m:
|
||
continue
|
||
az_ch = np.deg2rad(normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch]))))
|
||
alt = np.deg2rad(float(vertical_deg[ch]))
|
||
cos_alt = np.cos(alt)
|
||
xs.append(d_m * cos_alt * np.cos(az_ch))
|
||
ys.append(d_m * cos_alt * np.sin(az_ch))
|
||
zs.append(d_m * np.sin(alt))
|
||
if not xs:
|
||
return np.zeros((0, 3), dtype=np.float64)
|
||
return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
|
||
|
||
|
||
def _block_points_raw(
|
||
packet: bytes,
|
||
block_offset: int,
|
||
az_deg: float,
|
||
unit_mm: float,
|
||
vertical_deg: np.ndarray,
|
||
horizontal_deg: np.ndarray,
|
||
*,
|
||
min_range_m: float,
|
||
max_range_m: float,
|
||
) -> np.ndarray:
|
||
"""Return polar ``points_raw`` rows compatible with ``load_npz_xyz``."""
|
||
|
||
rows: list[list[float]] = []
|
||
idx = block_offset + 4
|
||
for ch in range(CHANNELS):
|
||
raw = read_u16_be(packet, idx)
|
||
intensity = float(packet[idx + 2])
|
||
idx += 3
|
||
if raw == 0:
|
||
continue
|
||
d_mm = float(raw) * unit_mm
|
||
d_m = d_mm * 0.001
|
||
if d_m < min_range_m or d_m > max_range_m:
|
||
continue
|
||
az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))
|
||
rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)])
|
||
if not rows:
|
||
return np.zeros((0, 5), dtype=np.float32)
|
||
return np.asarray(rows, dtype=np.float32)
|
||
|
||
|
||
def iter_h32_frames_polar(
|
||
capture: CaptureFile,
|
||
*,
|
||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||
frame_stride: int = 1,
|
||
min_range_m: float = 0.3,
|
||
max_range_m: float = 120.0,
|
||
max_points_per_frame: int | None = None,
|
||
vertical_deg: np.ndarray | None = None,
|
||
horizontal_deg: np.ndarray | None = None,
|
||
) -> list[LidarFramePolarExport]:
|
||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract."""
|
||
|
||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||
|
||
frames: list[LidarFramePolarExport] = []
|
||
point_chunks: list[np.ndarray] = []
|
||
t_start: float | None = None
|
||
t_end: float | None = None
|
||
host_ns = 0
|
||
prev_az: float | None = None
|
||
kept = 0
|
||
stride = max(1, int(frame_stride))
|
||
|
||
def emit() -> None:
|
||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||
if not point_chunks or t_start is None or t_end is None:
|
||
point_chunks = []
|
||
t_start = t_end = None
|
||
return
|
||
points = np.vstack(point_chunks)
|
||
point_chunks = []
|
||
start_s, end_s = t_start, t_end
|
||
frame_host = host_ns
|
||
t_start = t_end = None
|
||
if points.shape[0] < min_frame_points:
|
||
return
|
||
if kept % stride != 0:
|
||
kept += 1
|
||
return
|
||
kept += 1
|
||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||
points = points[select]
|
||
if end_s <= start_s:
|
||
end_s = start_s + 0.1
|
||
frames.append(
|
||
LidarFramePolarExport(
|
||
t_start_s=start_s,
|
||
t_end_s=end_s,
|
||
points_raw=points.astype(np.float32, copy=False),
|
||
host_receive_utc_ns=int(frame_host),
|
||
)
|
||
)
|
||
|
||
for chunk in capture.chunks:
|
||
packet = chunk.raw
|
||
if len(packet) != PACKET_LENGTH:
|
||
continue
|
||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||
unit = distance_unit_mm(packet)
|
||
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
|
||
idx = DATA_START
|
||
for _block in range(BLOCKS):
|
||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||
break
|
||
az = read_u16_be(packet, idx + 2) * 0.01
|
||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||
emit()
|
||
prev_az = az
|
||
pts = _block_points_raw(
|
||
packet,
|
||
idx,
|
||
az,
|
||
unit,
|
||
vertical,
|
||
horizontal,
|
||
min_range_m=min_range_m,
|
||
max_range_m=max_range_m,
|
||
)
|
||
if pts.shape[0]:
|
||
if t_start is None:
|
||
t_start = packet_t
|
||
t_end = packet_t
|
||
host_ns = chunk_host
|
||
point_chunks.append(pts)
|
||
idx += BLOCK_LENGTH
|
||
|
||
emit()
|
||
return frames
|
||
|
||
|
||
def iter_h32_frames(
|
||
capture: CaptureFile,
|
||
*,
|
||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||
frame_stride: int = 1,
|
||
min_range_m: float = 0.3,
|
||
max_range_m: float = 120.0,
|
||
max_points_per_frame: int | None = None,
|
||
vertical_deg: np.ndarray | None = None,
|
||
horizontal_deg: np.ndarray | None = None,
|
||
) -> list[LidarFrameExport]:
|
||
"""Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
|
||
|
||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||
|
||
frames: list[LidarFrameExport] = []
|
||
point_chunks: list[np.ndarray] = []
|
||
t_start: float | None = None
|
||
t_end: float | None = None
|
||
prev_az: float | None = None
|
||
kept = 0
|
||
stride = max(1, int(frame_stride))
|
||
|
||
def emit() -> None:
|
||
nonlocal point_chunks, t_start, t_end, kept
|
||
if not point_chunks or t_start is None or t_end is None:
|
||
point_chunks = []
|
||
t_start = t_end = None
|
||
return
|
||
points = np.vstack(point_chunks)
|
||
point_chunks = []
|
||
start_s, end_s = t_start, t_end
|
||
t_start = t_end = None
|
||
if points.shape[0] < min_frame_points:
|
||
return
|
||
if kept % stride != 0:
|
||
kept += 1
|
||
return
|
||
kept += 1
|
||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||
points = points[select]
|
||
if end_s <= start_s:
|
||
end_s = start_s + 0.1
|
||
frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
|
||
|
||
for chunk in capture.chunks:
|
||
packet = chunk.raw
|
||
if len(packet) != PACKET_LENGTH:
|
||
continue
|
||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||
unit = distance_unit_mm(packet)
|
||
idx = DATA_START
|
||
for _block in range(BLOCKS):
|
||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||
break
|
||
az = read_u16_be(packet, idx + 2) * 0.01
|
||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||
emit()
|
||
prev_az = az
|
||
pts = _block_points(
|
||
packet,
|
||
idx,
|
||
az,
|
||
unit,
|
||
vertical,
|
||
horizontal,
|
||
min_range_m=min_range_m,
|
||
max_range_m=max_range_m,
|
||
)
|
||
if pts.shape[0]:
|
||
if t_start is None:
|
||
t_start = packet_t
|
||
t_end = packet_t
|
||
point_chunks.append(pts)
|
||
idx += BLOCK_LENGTH
|
||
|
||
emit()
|
||
return frames
|