新增 N300/H32 rscap 到 V1 中间格式的导出工具与单元测试
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,104 @@
|
||||
"""Unit tests for rscap → V1 export helpers (no large real captures)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
|
||||
import numpy as np
|
||||
|
||||
from tools.rscap_v2.h32_msop import (
|
||||
CHANNELS,
|
||||
PACKET_LENGTH,
|
||||
decode_packet_points,
|
||||
default_vertical_deg,
|
||||
default_horizontal_deg,
|
||||
device_timestamp_ms,
|
||||
normalize_azimuth_deg,
|
||||
)
|
||||
from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples
|
||||
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
|
||||
|
||||
|
||||
def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
|
||||
packet = bytearray(PACKET_LENGTH)
|
||||
packet[17] = 1 # 2.5 mm unit
|
||||
sec = seconds.to_bytes(6, "big")
|
||||
packet[20:26] = sec
|
||||
packet[26:30] = int(microseconds).to_bytes(4, "big")
|
||||
az_raw = int(round(az_deg * 100))
|
||||
for block in range(12):
|
||||
offset = 42 + block * 100
|
||||
packet[offset] = 255
|
||||
packet[offset + 1] = 238
|
||||
packet[offset + 2] = (az_raw >> 8) & 0xFF
|
||||
packet[offset + 3] = az_raw & 0xFF
|
||||
idx = offset + 4
|
||||
for _ch in range(CHANNELS):
|
||||
# 4.0 m at 2.5 mm/unit => raw = 1600
|
||||
packet[idx] = (1600 >> 8) & 0xFF
|
||||
packet[idx + 1] = 1600 & 0xFF
|
||||
packet[idx + 2] = 10
|
||||
idx += 3
|
||||
return bytes(packet)
|
||||
|
||||
|
||||
def test_h32_device_timestamp_and_points():
|
||||
packet = _make_msop_packet(seconds=1700000000, microseconds=123000)
|
||||
assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123
|
||||
az_list, pts = decode_packet_points(
|
||||
packet,
|
||||
default_vertical_deg(),
|
||||
default_horizontal_deg(),
|
||||
min_range_m=0.1,
|
||||
max_range_m=50.0,
|
||||
)
|
||||
assert len(az_list) == 12
|
||||
assert pts.shape[0] == 12 * CHANNELS
|
||||
assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3)
|
||||
|
||||
|
||||
def test_normalize_azimuth():
|
||||
assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9
|
||||
|
||||
|
||||
def _n300_imu_frame(device_us: int = 123456) -> bytes:
|
||||
payload = bytearray(56)
|
||||
struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3)
|
||||
struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81)
|
||||
struct.pack_into("<q", payload, 48, device_us)
|
||||
header = bytearray([0xFC, 0x40, 56, 7])
|
||||
header.append(crc8_fdilink(header))
|
||||
crc = crc16_fdilink(payload)
|
||||
frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
|
||||
return frame
|
||||
|
||||
|
||||
def test_n300_imu_sample_from_capture_chunks():
|
||||
frame = _n300_imu_frame(654321)
|
||||
header = CaptureHeader(
|
||||
sensor_kind="wheeltec-n300",
|
||||
session_id="test",
|
||||
session_start_utc_ticks=0,
|
||||
session_start_monotonic_ticks=0,
|
||||
monotonic_frequency=10_000_000,
|
||||
port="COM1",
|
||||
baud=921600,
|
||||
file_start_utc_ticks=0,
|
||||
)
|
||||
chunk = RawChunk(
|
||||
sequence=1,
|
||||
receive_utc_ticks=100,
|
||||
receive_monotonic_ticks=1,
|
||||
raw=frame,
|
||||
record_file_offset=0,
|
||||
raw_file_offset=0,
|
||||
record_crc32=0,
|
||||
crc_valid=True,
|
||||
)
|
||||
capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
|
||||
samples = iter_n300_imu_samples(capture)
|
||||
assert len(samples) == 1
|
||||
assert samples[0].device_timestamp_us == 654321
|
||||
assert abs(samples[0].t_s - 654321e-6) < 1e-12
|
||||
assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
|
||||
assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5
|
||||
@@ -0,0 +1,191 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export N300 IMU + H32 MSOP V2 .rscap files to Lidar-IMU V1 intermediate format.
|
||||
|
||||
Output layout under --out:
|
||||
|
||||
imu.csv
|
||||
lidar/
|
||||
frames_index.csv
|
||||
frames/frame_XXXXX.npz
|
||||
export_summary.json
|
||||
|
||||
Timestamps written into the intermediate format are **device times**
|
||||
(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.h32_msop import iter_h32_frames
|
||||
from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays
|
||||
|
||||
|
||||
def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.writer(handle)
|
||||
writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"])
|
||||
for index in range(t.shape[0]):
|
||||
writer.writerow(
|
||||
[
|
||||
f"{t[index]:.9f}",
|
||||
f"{gyro[index, 0]:.12g}",
|
||||
f"{gyro[index, 1]:.12g}",
|
||||
f"{gyro[index, 2]:.12g}",
|
||||
f"{accel[index, 0]:.12g}",
|
||||
f"{accel[index, 1]:.12g}",
|
||||
f"{accel[index, 2]:.12g}",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def write_lidar_session(root: Path, frames) -> dict:
|
||||
frames_dir = root / "frames"
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
index_path = root / "frames_index.csv"
|
||||
with index_path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.writer(handle)
|
||||
writer.writerow(["frame_id", "filename", "t_start", "t_end"])
|
||||
point_counts = []
|
||||
for index, frame in enumerate(frames):
|
||||
rel = f"frames/frame_{index:05d}.npz"
|
||||
np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
|
||||
writer.writerow(
|
||||
[
|
||||
index,
|
||||
rel,
|
||||
f"{frame.t_start_s:.9f}",
|
||||
f"{frame.t_end_s:.9f}",
|
||||
]
|
||||
)
|
||||
point_counts.append(int(frame.points_xyz.shape[0]))
|
||||
return {
|
||||
"frames": len(frames),
|
||||
"points_min": int(min(point_counts)) if point_counts else 0,
|
||||
"points_max": int(max(point_counts)) if point_counts else 0,
|
||||
"points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
|
||||
"t_start": float(frames[0].t_start_s) if frames else None,
|
||||
"t_end": float(frames[-1].t_end_s) if frames else None,
|
||||
}
|
||||
|
||||
|
||||
def export_session(
|
||||
*,
|
||||
imu_rscap: Path,
|
||||
lidar_rscap: Path,
|
||||
out: Path,
|
||||
frame_stride: int = 1,
|
||||
max_points_per_frame: int | None = 80000,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
min_frame_points: int = 100,
|
||||
) -> dict:
|
||||
out.mkdir(parents=True, exist_ok=True)
|
||||
imu_capture = read_capture(imu_rscap)
|
||||
lidar_capture = read_capture(lidar_rscap)
|
||||
|
||||
samples = iter_n300_imu_samples(imu_capture)
|
||||
t, gyro, accel = samples_to_arrays(samples)
|
||||
imu_csv = out / "imu.csv"
|
||||
write_imu_csv(imu_csv, t, gyro, accel)
|
||||
|
||||
frames = iter_h32_frames(
|
||||
lidar_capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
)
|
||||
lidar_dir = out / "lidar"
|
||||
lidar_stats = write_lidar_session(lidar_dir, frames)
|
||||
|
||||
summary = {
|
||||
"imu_rscap": str(imu_rscap),
|
||||
"lidar_rscap": str(lidar_rscap),
|
||||
"out": str(out),
|
||||
"timestamp_policy": {
|
||||
"imu": "n300_device_timestamp_us -> seconds",
|
||||
"lidar": "h32_msop_device_timestamp_ms -> seconds (t_start/t_end per frame)",
|
||||
"host_utc": "not used as calibration timeline",
|
||||
},
|
||||
"imu": {
|
||||
"samples": int(t.shape[0]),
|
||||
"t_start": float(t[0]) if t.size else None,
|
||||
"t_end": float(t[-1]) if t.size else None,
|
||||
"capture": file_summary(imu_capture),
|
||||
},
|
||||
"lidar": {
|
||||
**lidar_stats,
|
||||
"frame_stride": int(frame_stride),
|
||||
"max_points_per_frame": max_points_per_frame,
|
||||
"capture": file_summary(lidar_capture),
|
||||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||
},
|
||||
"outputs": {
|
||||
"imu_csv": str(imu_csv),
|
||||
"lidar_session": str(lidar_dir),
|
||||
},
|
||||
}
|
||||
(out / "export_summary.json").write_text(
|
||||
json.dumps(summary, indent=2, ensure_ascii=False) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return summary
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap")
|
||||
parser.add_argument("--lidar-rscap", type=Path, required=True, help="H32 MSOP V2 .rscap")
|
||||
parser.add_argument("--out", type=Path, required=True, help="Output session directory")
|
||||
parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame")
|
||||
parser.add_argument(
|
||||
"--max-points-per-frame",
|
||||
type=int,
|
||||
default=80000,
|
||||
help="Uniform downsample cap per frame; 0 disables",
|
||||
)
|
||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||
parser.add_argument("--max-range-m", type=float, default=120.0)
|
||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||
args = parser.parse_args()
|
||||
max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame
|
||||
summary = export_session(
|
||||
imu_rscap=args.imu_rscap,
|
||||
lidar_rscap=args.lidar_rscap,
|
||||
out=args.out,
|
||||
frame_stride=args.frame_stride,
|
||||
max_points_per_frame=max_points,
|
||||
min_range_m=args.min_range_m,
|
||||
max_range_m=args.max_range_m,
|
||||
min_frame_points=args.min_frame_points,
|
||||
)
|
||||
print(json.dumps({
|
||||
"imu_samples": summary["imu"]["samples"],
|
||||
"lidar_frames": summary["lidar"]["frames"],
|
||||
"imu_csv": summary["outputs"]["imu_csv"],
|
||||
"lidar_session": summary["outputs"]["lidar_session"],
|
||||
"export_summary": str(Path(args.out) / "export_summary.json"),
|
||||
}, ensure_ascii=False, indent=2))
|
||||
if summary["imu"]["samples"] == 0:
|
||||
raise SystemExit("no valid N300 IMU samples decoded")
|
||||
if summary["lidar"]["frames"] == 0:
|
||||
raise SystemExit("no valid H32 frames decoded")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1 @@
|
||||
"""V2 .rscap readers and sensor decoders for export to V1 intermediate format."""
|
||||
@@ -0,0 +1,254 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import io
|
||||
import struct
|
||||
from dataclasses import dataclass, asdict
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
|
||||
FILE_MAGIC = "RAW_SERIAL_CAPTURE_FILE_V2"
|
||||
RECORD_MAGIC = "RAW_SERIAL_RECORD_V2"
|
||||
FOOTER_MAGIC = "RAW_SERIAL_CAPTURE_FOOTER_V2"
|
||||
|
||||
|
||||
def read_7bit_int(stream: BinaryIO) -> int:
|
||||
value = 0
|
||||
shift = 0
|
||||
while True:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated .NET string length")
|
||||
value |= (raw[0] & 0x7F) << shift
|
||||
if not raw[0] & 0x80:
|
||||
return value
|
||||
shift += 7
|
||||
if shift > 35:
|
||||
raise ValueError("invalid .NET string length")
|
||||
|
||||
|
||||
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||
length = read_7bit_int(stream)
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError("truncated .NET string")
|
||||
return raw.decode("utf-8")
|
||||
|
||||
|
||||
def read_i32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated int32")
|
||||
return struct.unpack("<i", raw)[0]
|
||||
|
||||
|
||||
def read_i64(stream: BinaryIO) -> int:
|
||||
raw = stream.read(8)
|
||||
if len(raw) != 8:
|
||||
raise EOFError("truncated int64")
|
||||
return struct.unpack("<q", raw)[0]
|
||||
|
||||
|
||||
def read_u32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated uint32")
|
||||
return struct.unpack("<I", raw)[0]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureHeader:
|
||||
sensor_kind: str
|
||||
session_id: str
|
||||
session_start_utc_ticks: int
|
||||
session_start_monotonic_ticks: int
|
||||
monotonic_frequency: int
|
||||
port: str
|
||||
baud: int
|
||||
file_start_utc_ticks: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RawChunk:
|
||||
sequence: int
|
||||
receive_utc_ticks: int
|
||||
receive_monotonic_ticks: int
|
||||
raw: bytes
|
||||
record_file_offset: int
|
||||
raw_file_offset: int
|
||||
record_crc32: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureFooter:
|
||||
clean_close: bool
|
||||
records: int
|
||||
bytes: int
|
||||
first_sequence: int
|
||||
last_sequence: int
|
||||
dropped_chunks: int
|
||||
dropped_bytes: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass
|
||||
class CaptureFile:
|
||||
path: str
|
||||
header: CaptureHeader
|
||||
chunks: list[RawChunk]
|
||||
footer: CaptureFooter | None
|
||||
truncated_tail: bool = False
|
||||
|
||||
|
||||
def read_header(stream: BinaryIO) -> CaptureHeader:
|
||||
if read_dotnet_string(stream) != FILE_MAGIC:
|
||||
raise ValueError("not a V2 raw capture file")
|
||||
version = read_i32(stream)
|
||||
if version != 2:
|
||||
raise ValueError(f"unsupported capture version: {version}")
|
||||
return CaptureHeader(
|
||||
sensor_kind=read_dotnet_string(stream),
|
||||
session_id=read_dotnet_string(stream),
|
||||
session_start_utc_ticks=read_i64(stream),
|
||||
session_start_monotonic_ticks=read_i64(stream),
|
||||
monotonic_frequency=read_i64(stream),
|
||||
port=read_dotnet_string(stream),
|
||||
baud=read_i32(stream),
|
||||
file_start_utc_ticks=read_i64(stream),
|
||||
)
|
||||
|
||||
|
||||
def parse_record_body(body: bytes, record_file_offset: int, record_crc: int) -> RawChunk:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != RECORD_MAGIC:
|
||||
raise ValueError("invalid record magic")
|
||||
sequence = read_i64(stream)
|
||||
receive_utc_ticks = read_i64(stream)
|
||||
receive_monotonic_ticks = read_i64(stream)
|
||||
raw_length = read_i32(stream)
|
||||
if raw_length < 0 or raw_length > 64 * 1024 * 1024:
|
||||
raise ValueError(f"invalid raw length: {raw_length}")
|
||||
raw_offset = record_file_offset + 4 + stream.tell()
|
||||
raw = stream.read(raw_length)
|
||||
if len(raw) != raw_length:
|
||||
raise EOFError("truncated raw bytes")
|
||||
crc_valid = (binascii.crc32(body) & 0xFFFFFFFF) == record_crc
|
||||
return RawChunk(
|
||||
sequence=sequence,
|
||||
receive_utc_ticks=receive_utc_ticks,
|
||||
receive_monotonic_ticks=receive_monotonic_ticks,
|
||||
raw=raw,
|
||||
record_file_offset=record_file_offset,
|
||||
raw_file_offset=raw_offset,
|
||||
record_crc32=record_crc,
|
||||
crc_valid=crc_valid,
|
||||
)
|
||||
|
||||
|
||||
def parse_footer(body: bytes, expected_crc: int) -> CaptureFooter:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != FOOTER_MAGIC:
|
||||
raise ValueError("invalid footer magic")
|
||||
clean_close = stream.read(1) == b"\x01"
|
||||
records = read_i64(stream)
|
||||
raw_bytes = read_i64(stream)
|
||||
first_sequence = read_i64(stream)
|
||||
last_sequence = read_i64(stream)
|
||||
dropped_chunks = read_i64(stream)
|
||||
dropped_bytes = read_i64(stream)
|
||||
return CaptureFooter(
|
||||
clean_close=clean_close,
|
||||
records=records,
|
||||
bytes=raw_bytes,
|
||||
first_sequence=first_sequence,
|
||||
last_sequence=last_sequence,
|
||||
dropped_chunks=dropped_chunks,
|
||||
dropped_bytes=dropped_bytes,
|
||||
crc_valid=(binascii.crc32(body) & 0xFFFFFFFF) == expected_crc,
|
||||
)
|
||||
|
||||
|
||||
def read_capture(path: Path) -> CaptureFile:
|
||||
chunks: list[RawChunk] = []
|
||||
footer = None
|
||||
truncated = False
|
||||
with path.open("rb") as stream:
|
||||
header = read_header(stream)
|
||||
while True:
|
||||
record_offset = stream.tell()
|
||||
length_raw = stream.read(4)
|
||||
if not length_raw:
|
||||
break
|
||||
if len(length_raw) != 4:
|
||||
truncated = True
|
||||
break
|
||||
length = struct.unpack("<i", length_raw)[0]
|
||||
try:
|
||||
if length == -1:
|
||||
footer_length = read_i32(stream)
|
||||
if footer_length < 0 or footer_length > 1024 * 1024:
|
||||
raise ValueError("invalid footer length")
|
||||
footer_body = stream.read(footer_length)
|
||||
if len(footer_body) != footer_length:
|
||||
raise EOFError("truncated footer")
|
||||
footer = parse_footer(footer_body, read_u32(stream))
|
||||
break
|
||||
if length <= 0 or length > 64 * 1024 * 1024:
|
||||
raise ValueError("invalid record length")
|
||||
body = stream.read(length)
|
||||
if len(body) != length:
|
||||
raise EOFError("truncated record body")
|
||||
record_crc = read_u32(stream)
|
||||
chunks.append(parse_record_body(body, record_offset, record_crc))
|
||||
except (EOFError, ValueError):
|
||||
truncated = True
|
||||
break
|
||||
return CaptureFile(str(path), header, chunks, footer, truncated)
|
||||
|
||||
|
||||
def sequence_gaps(chunks: list[RawChunk]) -> list[tuple[int, int, int]]:
|
||||
result = []
|
||||
for previous, current in zip(chunks, chunks[1:]):
|
||||
if current.sequence > previous.sequence + 1:
|
||||
result.append((previous.sequence, current.sequence, current.sequence - previous.sequence - 1))
|
||||
return result
|
||||
|
||||
|
||||
def file_summary(capture: CaptureFile) -> dict:
|
||||
gaps = sequence_gaps(capture.chunks)
|
||||
sequences = [chunk.sequence for chunk in capture.chunks]
|
||||
return {
|
||||
"path": capture.path,
|
||||
"sensor": capture.header.sensor_kind,
|
||||
"session_id": capture.header.session_id,
|
||||
"port": capture.header.port,
|
||||
"baud": capture.header.baud,
|
||||
"chunks_read": len(capture.chunks),
|
||||
"bytes_read": sum(len(chunk.raw) for chunk in capture.chunks),
|
||||
"first_sequence": sequences[0] if sequences else None,
|
||||
"last_sequence": sequences[-1] if sequences else None,
|
||||
"missing_chunks": sum(gap[2] for gap in gaps),
|
||||
"gap_count": len(gaps),
|
||||
"bad_record_crc": sum(not chunk.crc_valid for chunk in capture.chunks),
|
||||
"truncated_tail": capture.truncated_tail,
|
||||
"footer": None if capture.footer is None else asdict(capture.footer),
|
||||
"gaps": gaps[:100],
|
||||
}
|
||||
|
||||
|
||||
def iter_contiguous_segments(chunks: list[RawChunk]) -> Iterator[tuple[int, list[RawChunk]]]:
|
||||
if not chunks:
|
||||
return
|
||||
segment_id = 0
|
||||
current = [chunks[0]]
|
||||
for previous, chunk in zip(chunks, chunks[1:]):
|
||||
if chunk.sequence != previous.sequence + 1:
|
||||
yield segment_id, current
|
||||
segment_id += 1
|
||||
current = [chunk]
|
||||
else:
|
||||
current.append(chunk)
|
||||
yield segment_id, current
|
||||
|
||||
@@ -0,0 +1,224 @@
|
||||
"""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
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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 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
|
||||
@@ -0,0 +1,113 @@
|
||||
"""Decode Wheeltec N300 FDILink IMU frames from a V2 .rscap capture."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ImuSample:
|
||||
t_s: float
|
||||
gyro_rad_s: tuple[float, float, float]
|
||||
accel_m_s2: tuple[float, float, float]
|
||||
host_receive_utc_ticks: int
|
||||
device_timestamp_us: int
|
||||
|
||||
|
||||
def crc8_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
|
||||
return crc
|
||||
|
||||
|
||||
def crc16_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def _host_ticks_for_span(chunks: list[RawChunk], start: int, end: int) -> int:
|
||||
stream_offset = 0
|
||||
last = chunks[0]
|
||||
for chunk in chunks:
|
||||
next_offset = stream_offset + len(chunk.raw)
|
||||
if start < next_offset and end > stream_offset:
|
||||
last = chunk
|
||||
stream_offset = next_offset
|
||||
return last.receive_utc_ticks
|
||||
|
||||
|
||||
def iter_n300_imu_samples(capture: CaptureFile) -> list[ImuSample]:
|
||||
"""Return CRC-valid MSG_IMU (0x40) samples sorted by device timestamp."""
|
||||
|
||||
samples: list[ImuSample] = []
|
||||
expected_lengths = {0x40: 56, 0x41: 48}
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
start = stream.find(b"\xFC", cursor)
|
||||
if start < 0:
|
||||
break
|
||||
if start + 8 > len(stream):
|
||||
break
|
||||
payload_length = stream[start + 2]
|
||||
end = start + payload_length + 8
|
||||
if end > len(stream):
|
||||
if stream.find(b"\xFC", start + 1) < 0:
|
||||
break
|
||||
cursor = start + 1
|
||||
continue
|
||||
frame = stream[start:end]
|
||||
if frame[-1] != 0xFD:
|
||||
cursor = start + 1
|
||||
continue
|
||||
packet_id = frame[1]
|
||||
payload = frame[7:-1]
|
||||
header_ok = crc8_fdilink(frame[:4]) == frame[4]
|
||||
payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big")
|
||||
expected = expected_lengths.get(packet_id)
|
||||
length_ok = expected is None or len(payload) == expected
|
||||
if not (header_ok and payload_ok and length_ok):
|
||||
cursor = start + 1
|
||||
continue
|
||||
if packet_id == 0x40:
|
||||
gyro = struct.unpack_from("<3f", payload, 0)
|
||||
accel = struct.unpack_from("<3f", payload, 12)
|
||||
device_us = struct.unpack_from("<q", payload, 48)[0]
|
||||
samples.append(
|
||||
ImuSample(
|
||||
t_s=float(device_us) * 1e-6,
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
host_receive_utc_ticks=_host_ticks_for_span(chunks, start, end),
|
||||
device_timestamp_us=int(device_us),
|
||||
)
|
||||
)
|
||||
cursor = end
|
||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||
return samples
|
||||
|
||||
|
||||
def samples_to_arrays(samples: list[ImuSample]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
if not samples:
|
||||
return (
|
||||
np.zeros(0, dtype=np.float64),
|
||||
np.zeros((0, 3), dtype=np.float64),
|
||||
np.zeros((0, 3), dtype=np.float64),
|
||||
)
|
||||
t = np.asarray([sample.t_s for sample in samples], dtype=np.float64)
|
||||
gyro = np.asarray([sample.gyro_rad_s for sample in samples], dtype=np.float64)
|
||||
accel = np.asarray([sample.accel_m_s2 for sample in samples], dtype=np.float64)
|
||||
return t, gyro, accel
|
||||
Reference in New Issue
Block a user