新增雷达到RTK直接手眼标定流程
This commit is contained in:
lichun.qu
@@ -0,0 +1,22 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common import write_json
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("captures", nargs="+", type=Path)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
summaries = [file_summary(read_capture(path)) for path in args.captures]
|
||||
write_json(args.out, {"captures": summaries})
|
||||
for summary in summaries:
|
||||
print(summary)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -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,35 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common_corrected import parse_imu_capture, parse_rtk_capture, write_json, write_jsonl
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--rtk", type=Path, required=True)
|
||||
parser.add_argument("--imu", type=Path, required=True)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
args.out.mkdir(parents=True, exist_ok=True)
|
||||
rtk_capture = read_capture(args.rtk)
|
||||
imu_capture = read_capture(args.imu)
|
||||
rtk_rows = parse_rtk_capture(rtk_capture)
|
||||
imu_rows = parse_imu_capture(imu_capture)
|
||||
write_jsonl(args.out / "rtk.jsonl", rtk_rows)
|
||||
write_jsonl(args.out / "imu.jsonl", imu_rows)
|
||||
write_json(args.out / "parse_summary.json", {
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_records": len(rtk_rows),
|
||||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
||||
"imu_frames": len(imu_rows),
|
||||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
||||
})
|
||||
print(f"RTK records={len(rtk_rows)}, IMU frames={len(imu_rows)}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,300 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments, read_capture
|
||||
|
||||
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||||
|
||||
|
||||
def safe_float(value: str, default=None):
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def safe_int(value: str, default=None):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def nmea_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 3], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
value = 0
|
||||
for char in line[1:star]:
|
||||
value ^= ord(char)
|
||||
return value == expected
|
||||
|
||||
|
||||
def unicore_crc32(text: str) -> int:
|
||||
crc = 0
|
||||
for value in text.encode("ascii", "replace"):
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
||||
return crc & 0xFFFFFFFF
|
||||
|
||||
|
||||
def unicore_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0 or len(line) < star + 9:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 9], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
return unicore_crc32(line[1:star]) == expected
|
||||
|
||||
|
||||
def parse_checksum(line: str) -> bool:
|
||||
if line.startswith("$"):
|
||||
return nmea_checksum_valid(line)
|
||||
if line.startswith("#"):
|
||||
return unicore_checksum_valid(line)
|
||||
return False
|
||||
|
||||
|
||||
def parse_nmea_latlon(value: str, hemisphere: str):
|
||||
raw = safe_float(value)
|
||||
if raw is None:
|
||||
return None
|
||||
degrees = math.floor(raw / 100.0)
|
||||
result = degrees + (raw - degrees * 100.0) / 60.0
|
||||
if hemisphere.upper() in ("S", "W"):
|
||||
result = -result
|
||||
return result
|
||||
|
||||
|
||||
def parse_gga(line: str) -> dict:
|
||||
fields = line[:line.rfind("*")].split(",")
|
||||
if len(fields) < 10:
|
||||
raise ValueError("GGA has too few fields")
|
||||
return {
|
||||
"type": "GGA",
|
||||
"position_time_utc": fields[1],
|
||||
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
||||
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
||||
"fix_quality": safe_int(fields[6], -1),
|
||||
"satellites": safe_int(fields[7], -1),
|
||||
"hdop": safe_float(fields[8]),
|
||||
"altitude_m": safe_float(fields[9]),
|
||||
"geoid_separation_m": safe_float(fields[11]) if len(fields) > 11 else None,
|
||||
"differential_age_s": safe_float(fields[13]) if len(fields) > 13 else None,
|
||||
"station_id": fields[14].strip('"') if len(fields) > 14 else "",
|
||||
}
|
||||
|
||||
|
||||
def parse_heading(line: str) -> dict:
|
||||
before_crc = line[:line.rfind("*")]
|
||||
header, payload = before_crc.split(";", 1)
|
||||
header_fields = header.split(",")
|
||||
fields = payload.split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("UNIHEADINGA has too few fields")
|
||||
raw_heading = safe_float(fields[3])
|
||||
return {
|
||||
"type": "UNIHEADINGA",
|
||||
"gnss_week": safe_int(header_fields[4]) if len(header_fields) > 4 else None,
|
||||
"gnss_tow_ms": safe_int(header_fields[5]) if len(header_fields) > 5 else None,
|
||||
"heading_status": fields[0],
|
||||
"heading_solution": fields[1],
|
||||
"baseline_length_m": safe_float(fields[2]),
|
||||
"raw_heading_deg": raw_heading,
|
||||
"pitch_deg": safe_float(fields[4]),
|
||||
"heading_stddev_deg": safe_float(fields[6]),
|
||||
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
|
||||
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
|
||||
"satellites": safe_int(fields[9], -1) if len(fields) > 9 else -1,
|
||||
"solution_satellites": safe_int(fields[10], -1) if len(fields) > 10 else -1,
|
||||
"observations": safe_int(fields[11], -1) if len(fields) > 11 else -1,
|
||||
"multi_count": safe_int(fields[12], -1) if len(fields) > 12 else -1,
|
||||
"heading_valid": fields[0] == "SOL_COMPUTED" and fields[1] in {"NARROW_INT", "NARROW_FLOAT"},
|
||||
}
|
||||
|
||||
|
||||
def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict:
|
||||
first = chunks[0]
|
||||
last = chunks[-1]
|
||||
cursor = 0
|
||||
start_chunk = first
|
||||
end_chunk = last
|
||||
for chunk in chunks:
|
||||
chunk_start = cursor
|
||||
chunk_end = cursor + len(chunk.raw)
|
||||
if chunk_start <= offset < chunk_end:
|
||||
start_chunk = chunk
|
||||
if chunk_start < end <= chunk_end:
|
||||
end_chunk = chunk
|
||||
break
|
||||
cursor = chunk_end
|
||||
return {
|
||||
"source_segment_id": None,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + max(0, offset - sum(len(c.raw) for c in chunks if c.sequence < start_chunk.sequence)),
|
||||
"source_raw_byte_length": max(0, end - offset),
|
||||
}
|
||||
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
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):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
valid = parse_checksum(line)
|
||||
row = {
|
||||
"type": "UNKNOWN",
|
||||
"raw_line": line,
|
||||
"checksum_valid": valid,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": cursor - len(raw_line) - 1,
|
||||
"source_byte_length": len(raw_line) + 1,
|
||||
}
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def crc16_hi13(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 decode_hi91(frame: bytes) -> dict:
|
||||
f32 = lambda i: struct.unpack_from("<f", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x91,
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"air_pressure_pa": f32(10),
|
||||
"device_timestamp_ms": int.from_bytes(frame[14:18], "little"),
|
||||
"accel_x_mps2": f32(18) * 9.80665,
|
||||
"accel_y_mps2": f32(22) * 9.80665,
|
||||
"accel_z_mps2": f32(26) * 9.80665,
|
||||
"gyro_x_radps": f32(30) * math.pi / 180.0,
|
||||
"gyro_y_radps": f32(34) * math.pi / 180.0,
|
||||
"gyro_z_radps": f32(38) * math.pi / 180.0,
|
||||
"mag_x_ut": f32(42), "mag_y_ut": f32(46), "mag_z_ut": f32(50),
|
||||
"roll_deg": f32(54), "pitch_deg": f32(58), "yaw_deg": f32(62),
|
||||
"quaternion_w": f32(66), "quaternion_x": f32(70),
|
||||
"quaternion_y": f32(74), "quaternion_z": f32(78),
|
||||
}
|
||||
|
||||
|
||||
def decode_hi92(frame: bytes) -> dict:
|
||||
i16 = lambda i: struct.unpack_from("<h", frame, i)[0]
|
||||
i32 = lambda i: struct.unpack_from("<i", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x92,
|
||||
"status": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[10:12], "little"),
|
||||
"air_pressure_pa": i16(12) + 100000.0,
|
||||
"heave_m": i16(14) * 0.001,
|
||||
"gyro_x_radps": i16(16) * 0.001, "gyro_y_radps": i16(18) * 0.001, "gyro_z_radps": i16(20) * 0.001,
|
||||
"accel_x_mps2": i16(22) * 0.0048828, "accel_y_mps2": i16(24) * 0.0048828, "accel_z_mps2": i16(26) * 0.0048828,
|
||||
"mag_x_ut": i16(28) * 0.030517, "mag_y_ut": i16(30) * 0.030517, "mag_z_ut": i16(32) * 0.030517,
|
||||
"roll_deg": i32(34) * 0.001, "pitch_deg": i32(38) * 0.001, "yaw_deg": i32(42) * 0.001,
|
||||
"quaternion_w": i16(46) * 0.0001, "quaternion_x": i16(48) * 0.0001,
|
||||
"quaternion_y": i16(50) * 0.0001, "quaternion_z": i16(52) * 0.0001,
|
||||
}
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
source = chunk_source(chunks, start, end)
|
||||
source["source_segment_id"] = segment_id
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": start,
|
||||
"source_byte_length": frame_length,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
if expected == actual:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
|
||||
|
||||
def write_jsonl(path: Path, rows: Iterable[dict]) -> None:
|
||||
with path.open("w", encoding="utf-8", newline="\n") as stream:
|
||||
for row in rows:
|
||||
stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":")) + "\n")
|
||||
|
||||
|
||||
def write_json(path: Path, value: dict) -> None:
|
||||
path.write_text(json.dumps(value, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def load_jsonl(path: Path) -> list[dict]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
return [json.loads(line) for line in stream if line.strip()]
|
||||
@@ -0,0 +1,106 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
|
||||
from pipeline_common import *
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
_SPAN_CACHE: dict[int, tuple[list[RawChunk], list[int]]] = {}
|
||||
|
||||
|
||||
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
||||
key = id(chunks)
|
||||
cached = _SPAN_CACHE.get(key)
|
||||
if cached is not None and cached[0] is chunks:
|
||||
return cached[1]
|
||||
starts = []
|
||||
cursor = 0
|
||||
for chunk in chunks:
|
||||
starts.append(cursor)
|
||||
cursor += len(chunk.raw)
|
||||
_SPAN_CACHE[key] = (chunks, starts)
|
||||
return starts
|
||||
|
||||
|
||||
def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: int) -> dict:
|
||||
starts = _chunk_starts(chunks)
|
||||
start_index = max(0, min(len(chunks) - 1, bisect.bisect_right(starts, start) - 1))
|
||||
end_index = max(start_index, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
||||
start_chunk = chunks[start_index]
|
||||
end_chunk = chunks[end_index]
|
||||
return {
|
||||
"source_segment_id": segment_id,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + (start - starts[start_index]),
|
||||
"source_raw_byte_length": end - start,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(end_chunk.receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
|
||||
}
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
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):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
start = cursor
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
if row["crc_valid"]:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
Reference in New Issue
Block a user