rosotacom-dev 2.2__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- rosotacom/__init__.py +32 -0
- rosotacom/__main__.py +8 -0
- rosotacom/anonymize.py +360 -0
- rosotacom/bag_ground_truth.py +395 -0
- rosotacom/benched_set.py +492 -0
- rosotacom/benchmark.py +1842 -0
- rosotacom/bundle_check.py +517 -0
- rosotacom/cli.py +8637 -0
- rosotacom/cli_benchmark.py +8211 -0
- rosotacom/deployment.py +196 -0
- rosotacom/ffmpeg_packet.py +123 -0
- rosotacom/forensics.py +1172 -0
- rosotacom/geomap.py +848 -0
- rosotacom/network_profiles.py +563 -0
- rosotacom/network_shaper.py +127 -0
- rosotacom/plots.py +609 -0
- rosotacom/py.typed +0 -0
- rosotacom/resources/benched-set.yaml +317 -0
- rosotacom/resources/examples/.gitignore +1 -0
- rosotacom/resources/examples/README.md +163 -0
- rosotacom/resources/examples/__init__.py +1 -0
- rosotacom/resources/examples/bags/8_drop_reference/metadata.yaml +17 -0
- rosotacom/resources/examples/deployment.example.yaml +14 -0
- rosotacom/resources/examples/profiles.yaml +85 -0
- rosotacom/resources/examples/ros2docker.json +33 -0
- rosotacom/resources/examples/rosotacom.yaml +12 -0
- rosotacom/resources/examples/scenarios/2_native_chatter/scenario-definition.yaml +10 -0
- rosotacom/resources/examples/scripts/1_heartbeat/run_machine_a.sh +9 -0
- rosotacom/resources/examples/scripts/1_heartbeat/run_machine_b.sh +9 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_a/external.ros2docker.json +21 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_a/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_a/run_external.py +15 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_a/wait_for_echo_topic.sh +10 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_b/external.ros2docker.json +19 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_b/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/2_native_chatter/machine_b/run_external.py +15 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_a/external.ros2docker.json +12 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_a/external_session.yaml +9 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_a/foxglove.json +89 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_a/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_a/run_external.py +15 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_b/costmap_1second/costmap_1second_0.mcap +0 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_b/costmap_1second/metadata.yaml +44 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_b/external.ros2docker.json +13 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_b/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/3_comp_occ_grid/machine_b/run_external.py +15 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_a/external.ros2docker.json +12 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_a/external_session.yaml +9 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_a/foxglove.json +89 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_a/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_a/run_external.py +15 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_b/costmap_1second/costmap_1second_0.mcap +0 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_b/costmap_1second/metadata.yaml +44 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_b/external.ros2docker.json +13 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_b/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/4_comp_occ_grid_zen/machine_b/run_external.py +15 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_a/external.ros2docker.json +20 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_a/external_session.yaml +22 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_a/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_a/run_external.py +15 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_b/external.ros2docker.json +20 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_b/external_session.yaml +22 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_b/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/5_sized_payload/machine_b/run_external.py +15 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_a/external.ros2docker.json +18 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_a/external_session.yaml +22 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_a/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_a/run_external.py +15 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_b/external.ros2docker.json +18 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_b/external_session.yaml +22 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_b/run_communication.sh +9 -0
- rosotacom/resources/examples/scripts/6_sized_payload_zen/machine_b/run_external.py +15 -0
- rosotacom/resources/examples/sessions/10_restamp/session-definition.yaml +39 -0
- rosotacom/resources/examples/sessions/11_trickle/session-definition.yaml +42 -0
- rosotacom/resources/examples/sessions/12_content_integrity/session-definition.yaml +33 -0
- rosotacom/resources/examples/sessions/13_link_latency/session-definition.yaml +42 -0
- rosotacom/resources/examples/sessions/14_remote_assist_anonymized/session-definition.yaml +211 -0
- rosotacom/resources/examples/sessions/15_remote_assist_anonymized_costmap/session-definition.yaml +59 -0
- rosotacom/resources/examples/sessions/16_remote_assist_anonymized_camera/session-definition.yaml +61 -0
- rosotacom/resources/examples/sessions/17_synthetic_camera_quality/session-definition.yaml +57 -0
- rosotacom/resources/examples/sessions/1_heartbeat/session-definition.yaml +26 -0
- rosotacom/resources/examples/sessions/1_heartbeat_status/session-definition.yaml +37 -0
- rosotacom/resources/examples/sessions/2_native_chatter/session-definition.yaml +34 -0
- rosotacom/resources/examples/sessions/3_comp_occ_grid/session-definition.yaml +43 -0
- rosotacom/resources/examples/sessions/4_comp_occ_grid_zen/session-definition.yaml +43 -0
- rosotacom/resources/examples/sessions/5_sized_payload/session-definition.yaml +39 -0
- rosotacom/resources/examples/sessions/6_sized_payload_zen/session-definition.yaml +50 -0
- rosotacom/resources/examples/sessions/7_latched_static/session-definition.yaml +49 -0
- rosotacom/resources/examples/sessions/8_drop/session-definition.yaml +37 -0
- rosotacom/resources/examples/sessions/9_throttle/session-definition.yaml +37 -0
- rosotacom/resources/examples/sessions/bench_1_1_capacity/session-definition.yaml +42 -0
- rosotacom/resources/examples/sessions/bench_1_2_load_sweep/session-definition.yaml +51 -0
- rosotacom/resources/examples/sessions/bench_1_3_ramp/session-definition.yaml +41 -0
- rosotacom/resources/examples/sessions/bench_1_4_recovery/session-definition.yaml +47 -0
- rosotacom/resources/examples/sessions/bench_1_5_camera/session-definition.yaml +45 -0
- rosotacom/resources/ros2docker.json.example +33 -0
- rosotacom/resources/ws/.vscode/c_cpp_properties.json +20 -0
- rosotacom/resources/ws/.vscode/settings.json +8 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_easy_mode.xml.template +32 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_initial_peer_list.xml.template +21 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_v2.xml.template +17 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_v3.xml.template +48 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_v4.xml.template +95 -0
- rosotacom/resources/ws/ota_configs/archive/fastdds_v5.xml.template +64 -0
- rosotacom/resources/ws/ota_configs/create_zenoh_json5.py +56 -0
- rosotacom/resources/ws/ota_configs/cyclonedds_minimal.xml.template +9 -0
- rosotacom/resources/ws/ota_configs/cyclonedds_tuned.xml.template +33 -0
- rosotacom/resources/ws/ota_configs/fastdds_unicast.xml.template +71 -0
- rosotacom/resources/ws/ota_configs/get_ota_xml.py +214 -0
- rosotacom/resources/ws/ota_configs/utils.py +74 -0
- rosotacom/resources/ws/ota_configs/zenoh.json5.template +32 -0
- rosotacom/resources/ws/ros2src/.gitkeep +0 -0
- rosotacom/resources/ws/ros2src/com_cpp/CMakeLists.txt +67 -0
- rosotacom/resources/ws/ros2src/com_cpp/COLCON_IGNORE +0 -0
- rosotacom/resources/ws/ros2src/com_cpp/LICENSE.txt +25 -0
- rosotacom/resources/ws/ros2src/com_cpp/package.xml +37 -0
- rosotacom/resources/ws/ros2src/com_cpp/src/hz_monitor.cpp +192 -0
- rosotacom/resources/ws/ros2src/com_cpp/src/universal_compressor.cpp +283 -0
- rosotacom/resources/ws/ros2src/com_cpp/src/universal_decompressor.cpp +306 -0
- rosotacom/resources/ws/ros2src/com_msgs/CMakeLists.txt +22 -0
- rosotacom/resources/ws/ros2src/com_msgs/LICENSE.txt +25 -0
- rosotacom/resources/ws/ros2src/com_msgs/msg/CompressedData.msg +3 -0
- rosotacom/resources/ws/ros2src/com_msgs/msg/EchoHeartbeat.msg +10 -0
- rosotacom/resources/ws/ros2src/com_msgs/msg/OtaStamped.msg +4 -0
- rosotacom/resources/ws/ros2src/com_msgs/msg/SizedPayload.msg +4 -0
- rosotacom/resources/ws/ros2src/com_msgs/package.xml +25 -0
- rosotacom/resources/ws/ros2src/com_py/LICENSE.txt +25 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/__init__.py +0 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/base_bridge_relay.py +106 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/bridge_in.py +76 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/bridge_out.py +70 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/heartbeat_echo.py +221 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/hz_monitor.py +178 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/image_bandwidth_reducer.py +218 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/image_pixel_capper.py +370 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/lanelet_saver.py +80 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/latch_relay.py +205 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/costmap_remapper.py +73 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/marker_remapper.py +93 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/speed_test.py +107 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/tf_remapper.py +156 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/traffic_monitor.py +91 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/legacy/wifi_network_analysis_1.py +213 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/link_bytes.py +177 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/link_trace.py +186 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/local_global_frame_bridge.py +465 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/localization_map_split_reduce_saver.py +258 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/msg_transform.py +277 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/pair_management.py +87 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/pub_sub_pair.py +145 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/qos.py +254 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/relay_in.py +78 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/relay_out.py +75 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/restamp.py +191 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/sized_publisher.py +244 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/stage_latency.py +64 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/stage_latency_core.py +51 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/status_overview.py +476 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/status_overview_core.py +989 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/tf_map_to_cart_saver.py +99 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/topic_monitor.py +327 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/topic_resolution.py +171 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/trickle.py +183 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/universal_compressor.py +653 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/universal_decompressor.py +372 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/universal_ota_unwrapper.py +191 -0
- rosotacom/resources/ws/ros2src/com_py/com_py/universal_ota_wrapper.py +154 -0
- rosotacom/resources/ws/ros2src/com_py/package.xml +34 -0
- rosotacom/resources/ws/ros2src/com_py/resource/com_py +0 -0
- rosotacom/resources/ws/ros2src/com_py/setup.cfg +6 -0
- rosotacom/resources/ws/ros2src/com_py/setup.py +55 -0
- rosotacom/resources/ws/ros2src/com_py/test/test_copyright.py +25 -0
- rosotacom/resources/ws/ros2src/com_py/test/test_flake8.py +25 -0
- rosotacom/resources/ws/ros2src/com_py/test/test_pep257.py +23 -0
- rosotacom/resources/ws/session/content/base/session_plugin_base.yaml +633 -0
- rosotacom/resources/ws/session/content/network/up_connectivity.py +364 -0
- rosotacom/resources/ws/session/content/network/wifi_network_analysis_2.py +215 -0
- rosotacom/resources/ws/session/creation/anonymize_bag.py +156 -0
- rosotacom/resources/ws/session/creation/catmux_log_setup.sh +65 -0
- rosotacom/resources/ws/session/creation/create_session_yaml.py +145 -0
- rosotacom/resources/ws/session/creation/generate_session_files.py +3217 -0
- rosotacom/resources/ws/session/creation/run_session.py +447 -0
- rosotacom/resources/ws/session/creation/run_session_in_external_terminal.py +54 -0
- rosotacom/resources/ws/session/creation/strip_ansi.py +42 -0
- rosotacom/status_eval.py +475 -0
- rosotacom/stream_stats.py +650 -0
- rosotacom/trace_profiles.py +561 -0
- rosotacom/transit.py +249 -0
- rosotacom/video_quality.py +906 -0
- rosotacom_dev-2.2.dist-info/METADATA +874 -0
- rosotacom_dev-2.2.dist-info/RECORD +195 -0
- rosotacom_dev-2.2.dist-info/WHEEL +5 -0
- rosotacom_dev-2.2.dist-info/entry_points.txt +4 -0
- rosotacom_dev-2.2.dist-info/licenses/LICENSE.txt +25 -0
- rosotacom_dev-2.2.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,906 @@
|
|
|
1
|
+
"""Offline image-quality metrics for decoded camera frames.
|
|
2
|
+
|
|
3
|
+
The command layer accepts rosbag2 stage bags (including a bare or unfinalized
|
|
4
|
+
`.mcap` a killed recorder leaves behind) and simple JSON frame manifests. Metric
|
|
5
|
+
math is intentionally independent of ROS so unit tests can cover the alignment
|
|
6
|
+
and loss semantics without a runtime container.
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import base64
|
|
12
|
+
import json
|
|
13
|
+
import math
|
|
14
|
+
import sqlite3
|
|
15
|
+
import struct
|
|
16
|
+
from collections.abc import Iterable, Sequence
|
|
17
|
+
from dataclasses import dataclass
|
|
18
|
+
from io import BytesIO
|
|
19
|
+
from pathlib import Path
|
|
20
|
+
from statistics import median
|
|
21
|
+
from typing import Any, Literal
|
|
22
|
+
|
|
23
|
+
import yaml
|
|
24
|
+
|
|
25
|
+
Alignment = Literal["auto", "pts", "index"]
|
|
26
|
+
ResolvedAlignment = Literal["pts", "index"]
|
|
27
|
+
|
|
28
|
+
SENSOR_IMAGE_TYPE = "sensor_msgs/msg/Image"
|
|
29
|
+
SENSOR_COMPRESSED_IMAGE_TYPE = "sensor_msgs/msg/CompressedImage"
|
|
30
|
+
SUPPORTED_IMAGE_TOPIC_TYPES = {SENSOR_IMAGE_TYPE, SENSOR_COMPRESSED_IMAGE_TYPE}
|
|
31
|
+
SUPPORTED_ENCODINGS = {"mono8", "rgb8", "bgr8", "rgba8", "bgra8"}
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
@dataclass(frozen=True)
|
|
35
|
+
class VideoFrame:
|
|
36
|
+
index: int
|
|
37
|
+
pts: int | None
|
|
38
|
+
width: int
|
|
39
|
+
height: int
|
|
40
|
+
encoding: str
|
|
41
|
+
channels: int
|
|
42
|
+
data: bytes
|
|
43
|
+
recorded_time_ns: int | None = None
|
|
44
|
+
source: str | None = None
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def _align(offset: int, alignment: int) -> int:
|
|
48
|
+
return (offset + alignment - 1) // alignment * alignment
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
def _read_cdr_string(body: bytes, offset: int) -> tuple[str, int]:
|
|
52
|
+
offset = _align(offset, 4)
|
|
53
|
+
if offset + 4 > len(body):
|
|
54
|
+
raise ValueError("truncated CDR string length")
|
|
55
|
+
(length,) = struct.unpack_from("<I", body, offset)
|
|
56
|
+
offset += 4
|
|
57
|
+
end = offset + length
|
|
58
|
+
if end > len(body) or length == 0:
|
|
59
|
+
raise ValueError("truncated CDR string data")
|
|
60
|
+
value = body[offset : end - 1].decode("utf-8", errors="replace")
|
|
61
|
+
return value, end
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
def _canonicalize_image_data(width: int, height: int, encoding: str, step: int, raw: bytes) -> tuple[str, int, bytes]:
|
|
65
|
+
normalized = encoding.strip().lower()
|
|
66
|
+
if normalized not in SUPPORTED_ENCODINGS:
|
|
67
|
+
raise ValueError(
|
|
68
|
+
f"unsupported image encoding {encoding!r}; use a decoded sensor_msgs/Image stage with one of "
|
|
69
|
+
f"{sorted(SUPPORTED_ENCODINGS)}"
|
|
70
|
+
)
|
|
71
|
+
|
|
72
|
+
if normalized == "mono8":
|
|
73
|
+
channels = 1
|
|
74
|
+
canonical_encoding = "mono8"
|
|
75
|
+
elif normalized in {"rgb8", "bgr8"}:
|
|
76
|
+
channels = 3
|
|
77
|
+
canonical_encoding = "rgb8"
|
|
78
|
+
else:
|
|
79
|
+
channels = 4
|
|
80
|
+
canonical_encoding = "rgba8"
|
|
81
|
+
|
|
82
|
+
row_bytes = width * channels
|
|
83
|
+
if step < row_bytes:
|
|
84
|
+
raise ValueError(f"image step {step} is smaller than width*channels {row_bytes}")
|
|
85
|
+
if len(raw) < step * height:
|
|
86
|
+
raise ValueError(f"image data has {len(raw)} bytes, expected at least {step * height}")
|
|
87
|
+
|
|
88
|
+
rows: list[bytes] = []
|
|
89
|
+
for y in range(height):
|
|
90
|
+
row = raw[y * step : y * step + row_bytes]
|
|
91
|
+
if normalized == "bgr8":
|
|
92
|
+
converted = bytearray(row_bytes)
|
|
93
|
+
for x in range(0, row_bytes, 3):
|
|
94
|
+
converted[x : x + 3] = bytes((row[x + 2], row[x + 1], row[x]))
|
|
95
|
+
rows.append(bytes(converted))
|
|
96
|
+
elif normalized == "bgra8":
|
|
97
|
+
converted = bytearray(row_bytes)
|
|
98
|
+
for x in range(0, row_bytes, 4):
|
|
99
|
+
converted[x : x + 4] = bytes((row[x + 2], row[x + 1], row[x], row[x + 3]))
|
|
100
|
+
rows.append(bytes(converted))
|
|
101
|
+
else:
|
|
102
|
+
rows.append(bytes(row))
|
|
103
|
+
return canonical_encoding, channels, b"".join(rows)
|
|
104
|
+
|
|
105
|
+
|
|
106
|
+
def parse_sensor_image_cdr(
|
|
107
|
+
cdr: bytes,
|
|
108
|
+
*,
|
|
109
|
+
index: int = 0,
|
|
110
|
+
recorded_time_ns: int | None = None,
|
|
111
|
+
source: str | None = None,
|
|
112
|
+
) -> VideoFrame:
|
|
113
|
+
"""Parse serialized little-endian XCDR1 ``sensor_msgs/msg/Image`` bytes."""
|
|
114
|
+
if len(cdr) < 4 or cdr[1] != 0x01:
|
|
115
|
+
raise ValueError("expected little-endian CDR encapsulation (0x00 0x01 ...)")
|
|
116
|
+
body = cdr[4:]
|
|
117
|
+
offset = 0
|
|
118
|
+
if len(body) < 8:
|
|
119
|
+
raise ValueError("truncated sensor_msgs/Image header")
|
|
120
|
+
stamp_sec, stamp_nanosec = struct.unpack_from("<iI", body, offset)
|
|
121
|
+
offset += 8
|
|
122
|
+
_, offset = _read_cdr_string(body, offset) # header.frame_id
|
|
123
|
+
|
|
124
|
+
offset = _align(offset, 4)
|
|
125
|
+
if offset + 8 > len(body):
|
|
126
|
+
raise ValueError("truncated sensor_msgs/Image dimensions")
|
|
127
|
+
height, width = struct.unpack_from("<II", body, offset)
|
|
128
|
+
offset += 8
|
|
129
|
+
encoding, offset = _read_cdr_string(body, offset)
|
|
130
|
+
|
|
131
|
+
if offset >= len(body):
|
|
132
|
+
raise ValueError("truncated sensor_msgs/Image is_bigendian")
|
|
133
|
+
offset += 1 # is_bigendian, irrelevant for uint8 encodings
|
|
134
|
+
|
|
135
|
+
offset = _align(offset, 4)
|
|
136
|
+
if offset + 8 > len(body):
|
|
137
|
+
raise ValueError("truncated sensor_msgs/Image step/data length")
|
|
138
|
+
(step,) = struct.unpack_from("<I", body, offset)
|
|
139
|
+
offset += 4
|
|
140
|
+
(data_len,) = struct.unpack_from("<I", body, offset)
|
|
141
|
+
offset += 4
|
|
142
|
+
data = body[offset : offset + data_len]
|
|
143
|
+
if len(data) != data_len:
|
|
144
|
+
raise ValueError("truncated sensor_msgs/Image data")
|
|
145
|
+
|
|
146
|
+
canonical_encoding, channels, canonical_data = _canonicalize_image_data(width, height, encoding, step, data)
|
|
147
|
+
pts = stamp_sec * 1_000_000_000 + stamp_nanosec if stamp_sec or stamp_nanosec else None
|
|
148
|
+
return VideoFrame(
|
|
149
|
+
index=index,
|
|
150
|
+
pts=pts,
|
|
151
|
+
width=width,
|
|
152
|
+
height=height,
|
|
153
|
+
encoding=canonical_encoding,
|
|
154
|
+
channels=channels,
|
|
155
|
+
data=canonical_data,
|
|
156
|
+
recorded_time_ns=recorded_time_ns,
|
|
157
|
+
source=source,
|
|
158
|
+
)
|
|
159
|
+
|
|
160
|
+
|
|
161
|
+
def _decode_compressed_image_payload(payload: bytes, *, source: str | None = None) -> tuple[int, int, str, int, bytes]:
|
|
162
|
+
try:
|
|
163
|
+
from PIL import Image, UnidentifiedImageError
|
|
164
|
+
except ImportError:
|
|
165
|
+
raise RuntimeError(
|
|
166
|
+
"CompressedImage support requires the Python package `Pillow`; reinstall rosotacom or install Pillow"
|
|
167
|
+
) from None
|
|
168
|
+
|
|
169
|
+
try:
|
|
170
|
+
with Image.open(BytesIO(payload)) as image:
|
|
171
|
+
if image.mode in {"1", "L", "I;16", "I"}:
|
|
172
|
+
decoded = image.convert("L")
|
|
173
|
+
return decoded.width, decoded.height, "mono8", 1, decoded.tobytes()
|
|
174
|
+
decoded = image.convert("RGB")
|
|
175
|
+
return decoded.width, decoded.height, "rgb8", 3, decoded.tobytes()
|
|
176
|
+
except UnidentifiedImageError as exc:
|
|
177
|
+
where = f" from {source}" if source else ""
|
|
178
|
+
raise ValueError(f"unsupported CompressedImage payload{where}; expected JPEG or PNG bytes") from exc
|
|
179
|
+
|
|
180
|
+
|
|
181
|
+
def parse_sensor_compressed_image_cdr(
|
|
182
|
+
cdr: bytes,
|
|
183
|
+
*,
|
|
184
|
+
index: int = 0,
|
|
185
|
+
recorded_time_ns: int | None = None,
|
|
186
|
+
source: str | None = None,
|
|
187
|
+
) -> VideoFrame:
|
|
188
|
+
"""Parse serialized little-endian XCDR1 ``sensor_msgs/msg/CompressedImage`` bytes."""
|
|
189
|
+
if len(cdr) < 4 or cdr[1] != 0x01:
|
|
190
|
+
raise ValueError("expected little-endian CDR encapsulation (0x00 0x01 ...)")
|
|
191
|
+
body = cdr[4:]
|
|
192
|
+
offset = 0
|
|
193
|
+
if len(body) < 8:
|
|
194
|
+
raise ValueError("truncated sensor_msgs/CompressedImage header")
|
|
195
|
+
stamp_sec, stamp_nanosec = struct.unpack_from("<iI", body, offset)
|
|
196
|
+
offset += 8
|
|
197
|
+
_, offset = _read_cdr_string(body, offset) # header.frame_id
|
|
198
|
+
_, offset = _read_cdr_string(body, offset) # format, e.g. "jpeg" or "png"
|
|
199
|
+
|
|
200
|
+
offset = _align(offset, 4)
|
|
201
|
+
if offset + 4 > len(body):
|
|
202
|
+
raise ValueError("truncated sensor_msgs/CompressedImage data length")
|
|
203
|
+
(data_len,) = struct.unpack_from("<I", body, offset)
|
|
204
|
+
offset += 4
|
|
205
|
+
payload = body[offset : offset + data_len]
|
|
206
|
+
if len(payload) != data_len:
|
|
207
|
+
raise ValueError("truncated sensor_msgs/CompressedImage data")
|
|
208
|
+
|
|
209
|
+
width, height, encoding, channels, data = _decode_compressed_image_payload(payload, source=source)
|
|
210
|
+
pts = stamp_sec * 1_000_000_000 + stamp_nanosec if stamp_sec or stamp_nanosec else None
|
|
211
|
+
return VideoFrame(
|
|
212
|
+
index=index,
|
|
213
|
+
pts=pts,
|
|
214
|
+
width=width,
|
|
215
|
+
height=height,
|
|
216
|
+
encoding=encoding,
|
|
217
|
+
channels=channels,
|
|
218
|
+
data=data,
|
|
219
|
+
recorded_time_ns=recorded_time_ns,
|
|
220
|
+
source=source,
|
|
221
|
+
)
|
|
222
|
+
|
|
223
|
+
|
|
224
|
+
def _metadata_path(path: Path) -> Path | None:
|
|
225
|
+
if path.is_dir() and (path / "metadata.yaml").is_file():
|
|
226
|
+
return path / "metadata.yaml"
|
|
227
|
+
if path.is_file() and path.name == "metadata.yaml":
|
|
228
|
+
return path
|
|
229
|
+
return None
|
|
230
|
+
|
|
231
|
+
|
|
232
|
+
def _load_bag_metadata(path: Path) -> dict[str, Any]:
|
|
233
|
+
metadata = _metadata_path(path)
|
|
234
|
+
if metadata is None:
|
|
235
|
+
raise ValueError(f"not a rosbag2 directory or metadata.yaml: {path}")
|
|
236
|
+
loaded = yaml.safe_load(metadata.read_text(encoding="utf-8")) or {}
|
|
237
|
+
if not isinstance(loaded, dict):
|
|
238
|
+
raise ValueError(f"bag metadata must be a mapping: {metadata}")
|
|
239
|
+
info = loaded.get("rosbag2_bagfile_information") or {}
|
|
240
|
+
if not isinstance(info, dict):
|
|
241
|
+
raise ValueError(f"bag metadata missing rosbag2_bagfile_information: {metadata}")
|
|
242
|
+
return loaded
|
|
243
|
+
|
|
244
|
+
|
|
245
|
+
def _bag_base_dir(path: Path) -> Path:
|
|
246
|
+
metadata = _metadata_path(path)
|
|
247
|
+
if metadata is None:
|
|
248
|
+
raise ValueError(f"not a rosbag2 directory or metadata.yaml: {path}")
|
|
249
|
+
return metadata.parent
|
|
250
|
+
|
|
251
|
+
|
|
252
|
+
def _bag_info(metadata: dict[str, Any]) -> dict[str, Any]:
|
|
253
|
+
info = metadata.get("rosbag2_bagfile_information") or {}
|
|
254
|
+
if not isinstance(info, dict):
|
|
255
|
+
raise ValueError("bag metadata missing rosbag2_bagfile_information")
|
|
256
|
+
return info
|
|
257
|
+
|
|
258
|
+
|
|
259
|
+
def _bag_file_paths(path: Path, metadata: dict[str, Any], suffix: str) -> list[Path]:
|
|
260
|
+
base = _bag_base_dir(path)
|
|
261
|
+
info = _bag_info(metadata)
|
|
262
|
+
raw_paths = info.get("relative_file_paths") or []
|
|
263
|
+
if isinstance(raw_paths, list) and raw_paths:
|
|
264
|
+
files = [base / str(raw) for raw in raw_paths]
|
|
265
|
+
else:
|
|
266
|
+
files = sorted(base.glob(f"*{suffix}"))
|
|
267
|
+
if not files:
|
|
268
|
+
raise ValueError(f"bag has no {suffix} storage files: {base}")
|
|
269
|
+
return files
|
|
270
|
+
|
|
271
|
+
|
|
272
|
+
def _topic_type(metadata: dict[str, Any], topic: str) -> str | None:
|
|
273
|
+
topics = _bag_info(metadata).get("topics_with_message_count") or []
|
|
274
|
+
if not isinstance(topics, list):
|
|
275
|
+
return None
|
|
276
|
+
for entry in topics:
|
|
277
|
+
if not isinstance(entry, dict):
|
|
278
|
+
continue
|
|
279
|
+
topic_metadata = entry.get("topic_metadata") or {}
|
|
280
|
+
if not isinstance(topic_metadata, dict):
|
|
281
|
+
continue
|
|
282
|
+
if topic_metadata.get("name") == topic:
|
|
283
|
+
msg_type = topic_metadata.get("type")
|
|
284
|
+
return str(msg_type) if msg_type is not None else None
|
|
285
|
+
return None
|
|
286
|
+
|
|
287
|
+
|
|
288
|
+
def _require_supported_image_type(msg_type: str | None, topic: str, *, absent: str) -> str:
|
|
289
|
+
if msg_type is None:
|
|
290
|
+
raise ValueError(absent)
|
|
291
|
+
if msg_type not in SUPPORTED_IMAGE_TOPIC_TYPES:
|
|
292
|
+
raise ValueError(
|
|
293
|
+
f"topic {topic!r} has type {msg_type!r}; PSNR/SSIM requires {SENSOR_IMAGE_TYPE} "
|
|
294
|
+
f"or JPEG/PNG {SENSOR_COMPRESSED_IMAGE_TYPE} frames"
|
|
295
|
+
)
|
|
296
|
+
return msg_type
|
|
297
|
+
|
|
298
|
+
|
|
299
|
+
def _require_supported_image_topic(metadata: dict[str, Any], topic: str) -> str:
|
|
300
|
+
return _require_supported_image_type(
|
|
301
|
+
_topic_type(metadata, topic),
|
|
302
|
+
topic,
|
|
303
|
+
absent=f"topic {topic!r} is not present in bag metadata",
|
|
304
|
+
)
|
|
305
|
+
|
|
306
|
+
|
|
307
|
+
def _parse_image_frame_cdr(
|
|
308
|
+
msg_type: str,
|
|
309
|
+
cdr: bytes,
|
|
310
|
+
*,
|
|
311
|
+
index: int,
|
|
312
|
+
recorded_time_ns: int,
|
|
313
|
+
source: str,
|
|
314
|
+
) -> VideoFrame:
|
|
315
|
+
if msg_type == SENSOR_IMAGE_TYPE:
|
|
316
|
+
return parse_sensor_image_cdr(cdr, index=index, recorded_time_ns=recorded_time_ns, source=source)
|
|
317
|
+
if msg_type == SENSOR_COMPRESSED_IMAGE_TYPE:
|
|
318
|
+
return parse_sensor_compressed_image_cdr(cdr, index=index, recorded_time_ns=recorded_time_ns, source=source)
|
|
319
|
+
raise ValueError(f"unsupported image message type {msg_type!r}")
|
|
320
|
+
|
|
321
|
+
|
|
322
|
+
def _read_sqlite_bag(path: Path, metadata: dict[str, Any], topic: str) -> list[VideoFrame]:
|
|
323
|
+
msg_type = _require_supported_image_topic(metadata, topic)
|
|
324
|
+
rows: list[tuple[int, bytes]] = []
|
|
325
|
+
for db_path in _bag_file_paths(path, metadata, ".db3"):
|
|
326
|
+
conn = sqlite3.connect(db_path)
|
|
327
|
+
try:
|
|
328
|
+
topic_row = conn.execute("select id from topics where name = ?", (topic,)).fetchone()
|
|
329
|
+
if topic_row is None:
|
|
330
|
+
continue
|
|
331
|
+
topic_id = int(topic_row[0])
|
|
332
|
+
rows.extend(
|
|
333
|
+
(int(timestamp), bytes(data))
|
|
334
|
+
for timestamp, data in conn.execute(
|
|
335
|
+
"select timestamp, data from messages where topic_id = ? order by timestamp",
|
|
336
|
+
(topic_id,),
|
|
337
|
+
)
|
|
338
|
+
)
|
|
339
|
+
finally:
|
|
340
|
+
conn.close()
|
|
341
|
+
rows.sort(key=lambda row: row[0])
|
|
342
|
+
return [
|
|
343
|
+
_parse_image_frame_cdr(msg_type, data, index=index, recorded_time_ns=timestamp, source=f"{path}:{topic}")
|
|
344
|
+
for index, (timestamp, data) in enumerate(rows)
|
|
345
|
+
]
|
|
346
|
+
|
|
347
|
+
|
|
348
|
+
def _read_mcap_bag(path: Path, metadata: dict[str, Any], topic: str) -> list[VideoFrame]:
|
|
349
|
+
msg_type = _require_supported_image_topic(metadata, topic)
|
|
350
|
+
try:
|
|
351
|
+
from mcap.reader import make_reader
|
|
352
|
+
except ImportError:
|
|
353
|
+
raise RuntimeError(
|
|
354
|
+
"mcap support requires the Python package `mcap`; reinstall rosotacom or install mcap"
|
|
355
|
+
) from None
|
|
356
|
+
|
|
357
|
+
rows: list[tuple[int, bytes]] = []
|
|
358
|
+
for mcap_path in _bag_file_paths(path, metadata, ".mcap"):
|
|
359
|
+
with mcap_path.open("rb") as fp:
|
|
360
|
+
reader = make_reader(fp)
|
|
361
|
+
for _schema, channel, message in reader.iter_messages(topics=[topic]):
|
|
362
|
+
if channel.topic == topic:
|
|
363
|
+
rows.append((int(message.log_time), bytes(message.data)))
|
|
364
|
+
rows.sort(key=lambda row: row[0])
|
|
365
|
+
return [
|
|
366
|
+
_parse_image_frame_cdr(msg_type, data, index=index, recorded_time_ns=timestamp, source=f"{path}:{topic}")
|
|
367
|
+
for index, (timestamp, data) in enumerate(rows)
|
|
368
|
+
]
|
|
369
|
+
|
|
370
|
+
|
|
371
|
+
def read_rosbag_frames(path: str | Path, topic: str) -> list[VideoFrame]:
|
|
372
|
+
bag_path = Path(path)
|
|
373
|
+
metadata = _load_bag_metadata(bag_path)
|
|
374
|
+
storage_id = str(_bag_info(metadata).get("storage_identifier") or "").strip()
|
|
375
|
+
if storage_id == "sqlite3":
|
|
376
|
+
return _read_sqlite_bag(bag_path, metadata, topic)
|
|
377
|
+
if storage_id == "mcap":
|
|
378
|
+
return _read_mcap_bag(bag_path, metadata, topic)
|
|
379
|
+
raise ValueError(f"unsupported rosbag2 storage_identifier {storage_id!r}; expected sqlite3 or mcap")
|
|
380
|
+
|
|
381
|
+
|
|
382
|
+
def _stage_mcap_files(path: Path) -> list[Path]:
|
|
383
|
+
"""The `.mcap` split(s) of a metric stage bag, addressed as a bare file or a
|
|
384
|
+
directory that lacks a finalized `metadata.yaml` (e.g. a recorder killed at
|
|
385
|
+
session teardown)."""
|
|
386
|
+
if path.is_file() and path.suffix == ".mcap":
|
|
387
|
+
return [path]
|
|
388
|
+
if path.is_dir():
|
|
389
|
+
return sorted(p for p in path.glob("*.mcap") if p.is_file())
|
|
390
|
+
return []
|
|
391
|
+
|
|
392
|
+
|
|
393
|
+
def read_mcap_stage_frames(path: str | Path, topic: str) -> list[VideoFrame]:
|
|
394
|
+
"""Read image frames from a metric stage `.mcap` that has no rosbag2
|
|
395
|
+
`metadata.yaml`. rosbag2 writes `metadata.yaml` and the mcap summary/footer
|
|
396
|
+
only on clean shutdown; the metric recorder runs inside a container that is
|
|
397
|
+
torn down abruptly, so stage bags routinely arrive truncated. mcap is
|
|
398
|
+
self-describing, so the message type comes from the file's own Schema records
|
|
399
|
+
and a truncated tail is tolerated (the recorded prefix is kept)."""
|
|
400
|
+
try:
|
|
401
|
+
from mcap.exceptions import McapError
|
|
402
|
+
from mcap.records import Channel, Message, Schema
|
|
403
|
+
from mcap.stream_reader import StreamReader
|
|
404
|
+
except ImportError:
|
|
405
|
+
raise RuntimeError(
|
|
406
|
+
"mcap support requires the Python package `mcap`; reinstall rosotacom or install mcap"
|
|
407
|
+
) from None
|
|
408
|
+
|
|
409
|
+
stage_path = Path(path)
|
|
410
|
+
schemas: dict[int, str] = {}
|
|
411
|
+
channels: dict[int, tuple[str, int]] = {}
|
|
412
|
+
rows: list[tuple[int, int, bytes]] = []
|
|
413
|
+
for mcap_path in _stage_mcap_files(stage_path):
|
|
414
|
+
with mcap_path.open("rb") as fp:
|
|
415
|
+
records = iter(StreamReader(fp).records)
|
|
416
|
+
while True:
|
|
417
|
+
try:
|
|
418
|
+
record = next(records)
|
|
419
|
+
except StopIteration:
|
|
420
|
+
break
|
|
421
|
+
except (struct.error, EOFError, McapError):
|
|
422
|
+
# Unfinalized tail: keep what was decoded before the cut.
|
|
423
|
+
break
|
|
424
|
+
if isinstance(record, Schema):
|
|
425
|
+
schemas[record.id] = record.name
|
|
426
|
+
elif isinstance(record, Channel):
|
|
427
|
+
channels[record.id] = (record.topic, record.schema_id)
|
|
428
|
+
elif isinstance(record, Message):
|
|
429
|
+
channel = channels.get(record.channel_id)
|
|
430
|
+
if channel is not None and channel[0] == topic:
|
|
431
|
+
rows.append((int(record.log_time), channel[1], bytes(record.data)))
|
|
432
|
+
if not rows:
|
|
433
|
+
raise ValueError(f"topic {topic!r} has no messages in mcap stage bag {stage_path}")
|
|
434
|
+
msg_type = _require_supported_image_type(
|
|
435
|
+
schemas.get(rows[0][1]),
|
|
436
|
+
topic,
|
|
437
|
+
absent=f"topic {topic!r} has no schema in mcap stage bag {stage_path}",
|
|
438
|
+
)
|
|
439
|
+
rows.sort(key=lambda row: row[0])
|
|
440
|
+
return [
|
|
441
|
+
_parse_image_frame_cdr(msg_type, data, index=index, recorded_time_ns=timestamp, source=f"{stage_path}:{topic}")
|
|
442
|
+
for index, (timestamp, _schema_id, data) in enumerate(rows)
|
|
443
|
+
]
|
|
444
|
+
|
|
445
|
+
|
|
446
|
+
def _read_pnm(path: Path) -> tuple[int, int, str, int, bytes]:
|
|
447
|
+
raw = path.read_bytes()
|
|
448
|
+
offset = 0
|
|
449
|
+
|
|
450
|
+
def token() -> bytes:
|
|
451
|
+
nonlocal offset
|
|
452
|
+
while offset < len(raw) and raw[offset] in b" \t\r\n":
|
|
453
|
+
offset += 1
|
|
454
|
+
if offset < len(raw) and raw[offset] == ord("#"):
|
|
455
|
+
while offset < len(raw) and raw[offset] not in b"\r\n":
|
|
456
|
+
offset += 1
|
|
457
|
+
return token()
|
|
458
|
+
start = offset
|
|
459
|
+
while offset < len(raw) and raw[offset] not in b" \t\r\n":
|
|
460
|
+
offset += 1
|
|
461
|
+
if start == offset:
|
|
462
|
+
raise ValueError(f"invalid PNM header in {path}")
|
|
463
|
+
return raw[start:offset]
|
|
464
|
+
|
|
465
|
+
magic = token()
|
|
466
|
+
if magic not in {b"P5", b"P6"}:
|
|
467
|
+
raise ValueError(f"unsupported PNM magic {magic!r}; expected P5 or P6")
|
|
468
|
+
width = int(token())
|
|
469
|
+
height = int(token())
|
|
470
|
+
max_value = int(token())
|
|
471
|
+
if max_value != 255:
|
|
472
|
+
raise ValueError(f"unsupported PNM max value {max_value}; expected 255")
|
|
473
|
+
while offset < len(raw) and raw[offset] in b" \t\r\n":
|
|
474
|
+
offset += 1
|
|
475
|
+
channels = 1 if magic == b"P5" else 3
|
|
476
|
+
data = raw[offset:]
|
|
477
|
+
expected = width * height * channels
|
|
478
|
+
if len(data) != expected:
|
|
479
|
+
raise ValueError(f"PNM payload has {len(data)} bytes, expected {expected}: {path}")
|
|
480
|
+
return width, height, "mono8" if channels == 1 else "rgb8", channels, data
|
|
481
|
+
|
|
482
|
+
|
|
483
|
+
def read_frame_manifest(path: str | Path) -> list[VideoFrame]:
|
|
484
|
+
manifest_path = Path(path)
|
|
485
|
+
loaded = json.loads(manifest_path.read_text(encoding="utf-8"))
|
|
486
|
+
if not isinstance(loaded, dict) or loaded.get("schema_version") != 1:
|
|
487
|
+
raise ValueError(f"frame manifest must be a schema_version: 1 object: {manifest_path}")
|
|
488
|
+
frames_raw = loaded.get("frames")
|
|
489
|
+
if not isinstance(frames_raw, list):
|
|
490
|
+
raise ValueError(f"frame manifest must contain frames list: {manifest_path}")
|
|
491
|
+
|
|
492
|
+
frames: list[VideoFrame] = []
|
|
493
|
+
for index, raw in enumerate(frames_raw):
|
|
494
|
+
if not isinstance(raw, dict):
|
|
495
|
+
raise ValueError(f"frame manifest entry {index} must be a mapping")
|
|
496
|
+
pts = raw.get("pts")
|
|
497
|
+
pts_value = int(pts) if pts is not None else None
|
|
498
|
+
if raw.get("path") is not None:
|
|
499
|
+
frame_path = manifest_path.parent / str(raw["path"])
|
|
500
|
+
width, height, encoding, channels, data = _read_pnm(frame_path)
|
|
501
|
+
else:
|
|
502
|
+
width = int(raw["width"])
|
|
503
|
+
height = int(raw["height"])
|
|
504
|
+
encoding = str(raw.get("encoding") or "mono8")
|
|
505
|
+
channels = int(raw.get("channels") or (1 if encoding == "mono8" else 3))
|
|
506
|
+
data_b64 = raw.get("data_b64")
|
|
507
|
+
if not isinstance(data_b64, str):
|
|
508
|
+
raise ValueError(f"frame manifest entry {index} needs data_b64 or path")
|
|
509
|
+
data = base64.b64decode(data_b64)
|
|
510
|
+
canonical_encoding, canonical_channels, canonical_data = _canonicalize_image_data(
|
|
511
|
+
width,
|
|
512
|
+
height,
|
|
513
|
+
encoding,
|
|
514
|
+
width * channels,
|
|
515
|
+
data,
|
|
516
|
+
)
|
|
517
|
+
frames.append(
|
|
518
|
+
VideoFrame(
|
|
519
|
+
index=index,
|
|
520
|
+
pts=pts_value,
|
|
521
|
+
width=width,
|
|
522
|
+
height=height,
|
|
523
|
+
encoding=canonical_encoding,
|
|
524
|
+
channels=canonical_channels,
|
|
525
|
+
data=canonical_data,
|
|
526
|
+
source=str(manifest_path),
|
|
527
|
+
)
|
|
528
|
+
)
|
|
529
|
+
return frames
|
|
530
|
+
|
|
531
|
+
|
|
532
|
+
def load_frames(path: str | Path, *, topic: str | None = None) -> list[VideoFrame]:
|
|
533
|
+
frame_path = Path(path)
|
|
534
|
+
if _metadata_path(frame_path) is not None:
|
|
535
|
+
if not topic:
|
|
536
|
+
raise ValueError("--topic is required when reading rosbag2 inputs")
|
|
537
|
+
return read_rosbag_frames(frame_path, topic)
|
|
538
|
+
if _stage_mcap_files(frame_path):
|
|
539
|
+
if not topic:
|
|
540
|
+
raise ValueError("--topic is required when reading .mcap stage bags")
|
|
541
|
+
return read_mcap_stage_frames(frame_path, topic)
|
|
542
|
+
return read_frame_manifest(frame_path)
|
|
543
|
+
|
|
544
|
+
|
|
545
|
+
def _ensure_same_shape(reference: VideoFrame, degraded: VideoFrame) -> None:
|
|
546
|
+
if (
|
|
547
|
+
reference.width != degraded.width
|
|
548
|
+
or reference.height != degraded.height
|
|
549
|
+
or reference.channels != degraded.channels
|
|
550
|
+
or len(reference.data) != len(degraded.data)
|
|
551
|
+
):
|
|
552
|
+
raise ValueError(
|
|
553
|
+
"frame shape mismatch: "
|
|
554
|
+
f"ref={reference.width}x{reference.height}x{reference.channels} "
|
|
555
|
+
f"degraded={degraded.width}x{degraded.height}x{degraded.channels}"
|
|
556
|
+
)
|
|
557
|
+
|
|
558
|
+
|
|
559
|
+
def mean_squared_error(reference: bytes, degraded: bytes) -> float:
|
|
560
|
+
if len(reference) != len(degraded):
|
|
561
|
+
raise ValueError(f"sample length mismatch: {len(reference)} != {len(degraded)}")
|
|
562
|
+
if not reference:
|
|
563
|
+
raise ValueError("cannot compare empty frames")
|
|
564
|
+
total = 0
|
|
565
|
+
for ref_sample, deg_sample in zip(reference, degraded, strict=True):
|
|
566
|
+
delta = ref_sample - deg_sample
|
|
567
|
+
total += delta * delta
|
|
568
|
+
return total / len(reference)
|
|
569
|
+
|
|
570
|
+
|
|
571
|
+
def psnr_db(reference: bytes, degraded: bytes, *, max_value: float = 255.0) -> float:
|
|
572
|
+
mse = mean_squared_error(reference, degraded)
|
|
573
|
+
if mse == 0:
|
|
574
|
+
return math.inf
|
|
575
|
+
return 10.0 * math.log10((max_value * max_value) / mse)
|
|
576
|
+
|
|
577
|
+
|
|
578
|
+
def ssim(reference: bytes, degraded: bytes, *, max_value: float = 255.0) -> float:
|
|
579
|
+
if len(reference) != len(degraded):
|
|
580
|
+
raise ValueError(f"sample length mismatch: {len(reference)} != {len(degraded)}")
|
|
581
|
+
if not reference:
|
|
582
|
+
raise ValueError("cannot compare empty frames")
|
|
583
|
+
n = float(len(reference))
|
|
584
|
+
sum_x = sum(reference)
|
|
585
|
+
sum_y = sum(degraded)
|
|
586
|
+
mean_x = sum_x / n
|
|
587
|
+
mean_y = sum_y / n
|
|
588
|
+
var_x = sum((sample - mean_x) * (sample - mean_x) for sample in reference) / n
|
|
589
|
+
var_y = sum((sample - mean_y) * (sample - mean_y) for sample in degraded) / n
|
|
590
|
+
cov_xy = sum((x - mean_x) * (y - mean_y) for x, y in zip(reference, degraded, strict=True)) / n
|
|
591
|
+
c1 = (0.01 * max_value) ** 2
|
|
592
|
+
c2 = (0.03 * max_value) ** 2
|
|
593
|
+
numerator = (2.0 * mean_x * mean_y + c1) * (2.0 * cov_xy + c2)
|
|
594
|
+
denominator = (mean_x * mean_x + mean_y * mean_y + c1) * (var_x + var_y + c2)
|
|
595
|
+
return numerator / denominator
|
|
596
|
+
|
|
597
|
+
|
|
598
|
+
def _resolve_alignment(
|
|
599
|
+
reference: Sequence[VideoFrame],
|
|
600
|
+
degraded: Sequence[VideoFrame],
|
|
601
|
+
align: Alignment,
|
|
602
|
+
) -> ResolvedAlignment:
|
|
603
|
+
if align == "index":
|
|
604
|
+
return "index"
|
|
605
|
+
if align == "pts":
|
|
606
|
+
return "pts"
|
|
607
|
+
if (
|
|
608
|
+
reference
|
|
609
|
+
and degraded
|
|
610
|
+
and all(frame.pts is not None for frame in reference)
|
|
611
|
+
and all(frame.pts is not None for frame in degraded)
|
|
612
|
+
):
|
|
613
|
+
return "pts"
|
|
614
|
+
return "index"
|
|
615
|
+
|
|
616
|
+
|
|
617
|
+
def _unique_pts(frames: Sequence[VideoFrame], label: str) -> dict[int, VideoFrame]:
|
|
618
|
+
by_pts: dict[int, VideoFrame] = {}
|
|
619
|
+
for frame in frames:
|
|
620
|
+
if frame.pts is None:
|
|
621
|
+
raise ValueError(f"{label} frame {frame.index} has no pts; use --align index")
|
|
622
|
+
if frame.pts in by_pts:
|
|
623
|
+
raise ValueError(f"{label} has duplicate pts {frame.pts}; use --align index")
|
|
624
|
+
by_pts[frame.pts] = frame
|
|
625
|
+
return by_pts
|
|
626
|
+
|
|
627
|
+
|
|
628
|
+
def _metric_value(value: float, digits: int = 6) -> float | str:
|
|
629
|
+
if math.isinf(value):
|
|
630
|
+
return "inf"
|
|
631
|
+
return round(value, digits)
|
|
632
|
+
|
|
633
|
+
|
|
634
|
+
def _percentile(values: Sequence[float], percentile: float) -> float:
|
|
635
|
+
if not values:
|
|
636
|
+
raise ValueError("percentile requires at least one value")
|
|
637
|
+
ordered = sorted(values)
|
|
638
|
+
if len(ordered) == 1:
|
|
639
|
+
return ordered[0]
|
|
640
|
+
position = (len(ordered) - 1) * percentile
|
|
641
|
+
lower = math.floor(position)
|
|
642
|
+
upper = math.ceil(position)
|
|
643
|
+
if lower == upper:
|
|
644
|
+
return ordered[int(position)]
|
|
645
|
+
fraction = position - lower
|
|
646
|
+
return ordered[lower] * (1.0 - fraction) + ordered[upper] * fraction
|
|
647
|
+
|
|
648
|
+
|
|
649
|
+
def _metric_summary(values: Sequence[float]) -> dict[str, float | str | None]:
|
|
650
|
+
if not values:
|
|
651
|
+
return {"mean": None, "median": None, "p5": None, "min": None}
|
|
652
|
+
return {
|
|
653
|
+
"mean": _metric_value(sum(values) / len(values)),
|
|
654
|
+
"median": _metric_value(median(values)),
|
|
655
|
+
"p5": _metric_value(_percentile(values, 0.05)),
|
|
656
|
+
"min": _metric_value(min(values)),
|
|
657
|
+
}
|
|
658
|
+
|
|
659
|
+
|
|
660
|
+
def _compare_pair(reference: VideoFrame, degraded: VideoFrame) -> dict[str, Any]:
|
|
661
|
+
_ensure_same_shape(reference, degraded)
|
|
662
|
+
frame_psnr = psnr_db(reference.data, degraded.data)
|
|
663
|
+
frame_ssim = ssim(reference.data, degraded.data)
|
|
664
|
+
return {
|
|
665
|
+
"status": "compared",
|
|
666
|
+
"ref_index": reference.index,
|
|
667
|
+
"degraded_index": degraded.index,
|
|
668
|
+
"pts": reference.pts,
|
|
669
|
+
"recorded_time_ns": reference.recorded_time_ns,
|
|
670
|
+
"width": reference.width,
|
|
671
|
+
"height": reference.height,
|
|
672
|
+
"encoding": reference.encoding,
|
|
673
|
+
"psnr_db": _metric_value(frame_psnr),
|
|
674
|
+
"ssim": _metric_value(frame_ssim),
|
|
675
|
+
"_psnr_raw": frame_psnr,
|
|
676
|
+
"_ssim_raw": frame_ssim,
|
|
677
|
+
}
|
|
678
|
+
|
|
679
|
+
|
|
680
|
+
def _lost_frame(reference: VideoFrame) -> dict[str, Any]:
|
|
681
|
+
return {
|
|
682
|
+
"status": "lost",
|
|
683
|
+
"ref_index": reference.index,
|
|
684
|
+
"degraded_index": None,
|
|
685
|
+
"pts": reference.pts,
|
|
686
|
+
"recorded_time_ns": reference.recorded_time_ns,
|
|
687
|
+
"width": reference.width,
|
|
688
|
+
"height": reference.height,
|
|
689
|
+
"encoding": reference.encoding,
|
|
690
|
+
"psnr_db": None,
|
|
691
|
+
"ssim": None,
|
|
692
|
+
}
|
|
693
|
+
|
|
694
|
+
|
|
695
|
+
def _extra_frame(degraded: VideoFrame) -> dict[str, Any]:
|
|
696
|
+
return {
|
|
697
|
+
"degraded_index": degraded.index,
|
|
698
|
+
"pts": degraded.pts,
|
|
699
|
+
"recorded_time_ns": degraded.recorded_time_ns,
|
|
700
|
+
"width": degraded.width,
|
|
701
|
+
"height": degraded.height,
|
|
702
|
+
"encoding": degraded.encoding,
|
|
703
|
+
}
|
|
704
|
+
|
|
705
|
+
|
|
706
|
+
def _strip_private_metrics(rows: Iterable[dict[str, Any]]) -> list[dict[str, Any]]:
|
|
707
|
+
stripped = []
|
|
708
|
+
for row in rows:
|
|
709
|
+
clean = dict(row)
|
|
710
|
+
clean.pop("_psnr_raw", None)
|
|
711
|
+
clean.pop("_ssim_raw", None)
|
|
712
|
+
stripped.append(clean)
|
|
713
|
+
return stripped
|
|
714
|
+
|
|
715
|
+
|
|
716
|
+
def compare_frame_sequences(
|
|
717
|
+
reference: Sequence[VideoFrame],
|
|
718
|
+
degraded: Sequence[VideoFrame],
|
|
719
|
+
*,
|
|
720
|
+
align: Alignment = "auto",
|
|
721
|
+
) -> dict[str, Any]:
|
|
722
|
+
resolved_align = _resolve_alignment(reference, degraded, align)
|
|
723
|
+
rows: list[dict[str, Any]] = []
|
|
724
|
+
extras: list[dict[str, Any]] = []
|
|
725
|
+
|
|
726
|
+
if resolved_align == "pts":
|
|
727
|
+
degraded_by_pts = _unique_pts(degraded, "degraded")
|
|
728
|
+
reference_by_pts = _unique_pts(reference, "reference")
|
|
729
|
+
for ref in reference:
|
|
730
|
+
assert ref.pts is not None
|
|
731
|
+
deg = degraded_by_pts.get(ref.pts)
|
|
732
|
+
rows.append(_compare_pair(ref, deg) if deg is not None else _lost_frame(ref))
|
|
733
|
+
for deg in degraded:
|
|
734
|
+
assert deg.pts is not None
|
|
735
|
+
if deg.pts not in reference_by_pts:
|
|
736
|
+
extras.append(_extra_frame(deg))
|
|
737
|
+
else:
|
|
738
|
+
for index, ref in enumerate(reference):
|
|
739
|
+
rows.append(_compare_pair(ref, degraded[index]) if index < len(degraded) else _lost_frame(ref))
|
|
740
|
+
extras = [_extra_frame(frame) for frame in degraded[len(reference) :]]
|
|
741
|
+
|
|
742
|
+
compared = [row for row in rows if row["status"] == "compared"]
|
|
743
|
+
lost = [row for row in rows if row["status"] == "lost"]
|
|
744
|
+
psnr_values = [float(row["_psnr_raw"]) for row in compared]
|
|
745
|
+
ssim_values = [float(row["_ssim_raw"]) for row in compared]
|
|
746
|
+
worst_row = min(compared, key=lambda row: (float(row["_ssim_raw"]), float(row["_psnr_raw"])), default=None)
|
|
747
|
+
worst_frame = None if worst_row is None else _strip_private_metrics([worst_row])[0]
|
|
748
|
+
delivery_ratio = len(compared) / len(reference) if reference else 1.0
|
|
749
|
+
loss_pct = (len(lost) / len(reference) * 100.0) if reference else 0.0
|
|
750
|
+
|
|
751
|
+
return {
|
|
752
|
+
"schema_version": 1,
|
|
753
|
+
"alignment": resolved_align,
|
|
754
|
+
"delivery": {
|
|
755
|
+
"reference_frames": len(reference),
|
|
756
|
+
"degraded_frames": len(degraded),
|
|
757
|
+
"compared_frames": len(compared),
|
|
758
|
+
"lost_frames": len(lost),
|
|
759
|
+
"extra_degraded_frames": len(extras),
|
|
760
|
+
"delivery_ratio": round(delivery_ratio, 6),
|
|
761
|
+
"loss_pct": round(loss_pct, 6),
|
|
762
|
+
},
|
|
763
|
+
"summary": {
|
|
764
|
+
"psnr_db": _metric_summary(psnr_values),
|
|
765
|
+
"ssim": _metric_summary(ssim_values),
|
|
766
|
+
"worst_frame": worst_frame,
|
|
767
|
+
},
|
|
768
|
+
"frames": _strip_private_metrics(rows),
|
|
769
|
+
"extra_degraded_frames": extras,
|
|
770
|
+
}
|
|
771
|
+
|
|
772
|
+
|
|
773
|
+
def compare_inputs(
|
|
774
|
+
reference: str | Path,
|
|
775
|
+
degraded: str | Path,
|
|
776
|
+
*,
|
|
777
|
+
topic: str | None,
|
|
778
|
+
reference_topic: str | None = None,
|
|
779
|
+
degraded_topic: str | None = None,
|
|
780
|
+
align: Alignment,
|
|
781
|
+
) -> dict[str, Any]:
|
|
782
|
+
ref_topic = reference_topic or topic
|
|
783
|
+
deg_topic = degraded_topic or topic
|
|
784
|
+
return compare_frame_sequences(
|
|
785
|
+
load_frames(reference, topic=ref_topic),
|
|
786
|
+
load_frames(degraded, topic=deg_topic),
|
|
787
|
+
align=align,
|
|
788
|
+
)
|
|
789
|
+
|
|
790
|
+
|
|
791
|
+
def _summary_metric_as_float(value: float | str | None) -> float | None:
|
|
792
|
+
if value == "inf":
|
|
793
|
+
return math.inf
|
|
794
|
+
if isinstance(value, int | float):
|
|
795
|
+
return float(value)
|
|
796
|
+
return None
|
|
797
|
+
|
|
798
|
+
|
|
799
|
+
def threshold_failures(
|
|
800
|
+
report: dict[str, Any],
|
|
801
|
+
*,
|
|
802
|
+
min_mean_psnr: float | None = None,
|
|
803
|
+
min_mean_ssim: float | None = None,
|
|
804
|
+
max_loss_pct: float | None = None,
|
|
805
|
+
) -> list[str]:
|
|
806
|
+
failures: list[str] = []
|
|
807
|
+
summary = report.get("summary") or {}
|
|
808
|
+
delivery = report.get("delivery") or {}
|
|
809
|
+
psnr_mean = _summary_metric_as_float((summary.get("psnr_db") or {}).get("mean"))
|
|
810
|
+
ssim_mean = _summary_metric_as_float((summary.get("ssim") or {}).get("mean"))
|
|
811
|
+
loss_pct = _summary_metric_as_float(delivery.get("loss_pct"))
|
|
812
|
+
if min_mean_psnr is not None and (psnr_mean is None or psnr_mean < min_mean_psnr):
|
|
813
|
+
failures.append(f"mean PSNR {psnr_mean} dB is below {min_mean_psnr} dB")
|
|
814
|
+
if min_mean_ssim is not None and (ssim_mean is None or ssim_mean < min_mean_ssim):
|
|
815
|
+
failures.append(f"mean SSIM {ssim_mean} is below {min_mean_ssim}")
|
|
816
|
+
if max_loss_pct is not None and (loss_pct is None or loss_pct > max_loss_pct):
|
|
817
|
+
failures.append(f"loss {loss_pct}% exceeds {max_loss_pct}%")
|
|
818
|
+
return failures
|
|
819
|
+
|
|
820
|
+
|
|
821
|
+
def synthetic_frame(index: int, *, width: int, height: int, channels: int = 1, seed: int = 0) -> VideoFrame:
|
|
822
|
+
if channels not in {1, 3}:
|
|
823
|
+
raise ValueError("synthetic frames support 1 or 3 channels")
|
|
824
|
+
values = bytearray(width * height * channels)
|
|
825
|
+
offset = 0
|
|
826
|
+
for y in range(height):
|
|
827
|
+
for x in range(width):
|
|
828
|
+
base = (x * 7 + y * 13 + index * 17 + seed * 31) % 256
|
|
829
|
+
if channels == 1:
|
|
830
|
+
values[offset] = base
|
|
831
|
+
offset += 1
|
|
832
|
+
else:
|
|
833
|
+
values[offset : offset + 3] = bytes((base, (base + 53) % 256, (base + 101) % 256))
|
|
834
|
+
offset += 3
|
|
835
|
+
return VideoFrame(
|
|
836
|
+
index=index,
|
|
837
|
+
pts=index,
|
|
838
|
+
width=width,
|
|
839
|
+
height=height,
|
|
840
|
+
encoding="mono8" if channels == 1 else "rgb8",
|
|
841
|
+
channels=channels,
|
|
842
|
+
data=bytes(values),
|
|
843
|
+
)
|
|
844
|
+
|
|
845
|
+
|
|
846
|
+
def degrade_frame(frame: VideoFrame, *, quantization_step: int) -> VideoFrame:
|
|
847
|
+
if quantization_step <= 1:
|
|
848
|
+
degraded = frame.data
|
|
849
|
+
else:
|
|
850
|
+
degraded = bytes((sample // quantization_step) * quantization_step for sample in frame.data)
|
|
851
|
+
return VideoFrame(
|
|
852
|
+
index=frame.index,
|
|
853
|
+
pts=frame.pts,
|
|
854
|
+
width=frame.width,
|
|
855
|
+
height=frame.height,
|
|
856
|
+
encoding=frame.encoding,
|
|
857
|
+
channels=frame.channels,
|
|
858
|
+
data=degraded,
|
|
859
|
+
recorded_time_ns=frame.recorded_time_ns,
|
|
860
|
+
source=frame.source,
|
|
861
|
+
)
|
|
862
|
+
|
|
863
|
+
|
|
864
|
+
def write_frame_manifest(frames: Sequence[VideoFrame], path: str | Path) -> Path:
|
|
865
|
+
out = Path(path)
|
|
866
|
+
out.parent.mkdir(parents=True, exist_ok=True)
|
|
867
|
+
payload = {
|
|
868
|
+
"schema_version": 1,
|
|
869
|
+
"frames": [
|
|
870
|
+
{
|
|
871
|
+
"index": frame.index,
|
|
872
|
+
"pts": frame.pts,
|
|
873
|
+
"width": frame.width,
|
|
874
|
+
"height": frame.height,
|
|
875
|
+
"encoding": frame.encoding,
|
|
876
|
+
"channels": frame.channels,
|
|
877
|
+
"data_b64": base64.b64encode(frame.data).decode("ascii"),
|
|
878
|
+
}
|
|
879
|
+
for frame in frames
|
|
880
|
+
],
|
|
881
|
+
}
|
|
882
|
+
out.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n", encoding="utf-8")
|
|
883
|
+
return out
|
|
884
|
+
|
|
885
|
+
|
|
886
|
+
def write_synthetic_pair(
|
|
887
|
+
output_dir: str | Path,
|
|
888
|
+
*,
|
|
889
|
+
frames: int = 12,
|
|
890
|
+
width: int = 32,
|
|
891
|
+
height: int = 24,
|
|
892
|
+
channels: int = 1,
|
|
893
|
+
seed: int = 0,
|
|
894
|
+
quantization_step: int = 8,
|
|
895
|
+
drop_every: int | None = None,
|
|
896
|
+
) -> tuple[Path, Path]:
|
|
897
|
+
out_dir = Path(output_dir)
|
|
898
|
+
reference = [synthetic_frame(i, width=width, height=height, channels=channels, seed=seed) for i in range(frames)]
|
|
899
|
+
degraded = []
|
|
900
|
+
for frame in reference:
|
|
901
|
+
if drop_every is not None and drop_every > 0 and (frame.index + 1) % drop_every == 0:
|
|
902
|
+
continue
|
|
903
|
+
degraded.append(degrade_frame(frame, quantization_step=quantization_step))
|
|
904
|
+
ref_path = write_frame_manifest(reference, out_dir / "reference-frames.json")
|
|
905
|
+
degraded_path = write_frame_manifest(degraded, out_dir / "degraded-frames.json")
|
|
906
|
+
return ref_path, degraded_path
|