-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathdecoder.py
More file actions
260 lines (217 loc) · 9.29 KB
/
Copy pathdecoder.py
File metadata and controls
260 lines (217 loc) · 9.29 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
from __future__ import annotations
import multiprocessing
import os
from typing import Any, Callable, Optional, Sequence
import av
import cv2
from mcap.exceptions import DecoderNotFoundError
from mcap.reader import make_reader
from mcap.records import Channel, Message, Schema
encode_type_map = {
1: "raw",
2: "h264",
3: "h265",
4: "jpeg",
}
def _topic_matches(topic: str, filters: Optional[Sequence[str]]) -> bool:
if not filters:
return True
return any(topic.endswith(item) for item in filters)
def _sanitize_topic(topic: str) -> str:
parts = topic.split("compressed/camera")
if len(parts) > 1:
topic = parts[-1]
else:
topic = parts[0]
return topic.strip("/").replace("/", "_")
def _stamp_to_ms(stamp: Any) -> int:
seconds = getattr(stamp, "seconds", getattr(stamp, "sec", None))
nanos = getattr(stamp, "nanos", getattr(stamp, "nanosec", None))
if seconds is None or nanos is None:
raise AttributeError("stamp missing seconds/nanos fields")
return int(seconds * 1e3 + nanos / 1e6)
def _has_sps_pps(data: bytes, hevc: bool) -> bool:
start_codes = (b"\x00\x00\x01", b"\x00\x00\x00\x01")
i = 0
length = len(data)
while i < length:
next_positions = [pos for sc in start_codes if (pos := data.find(sc, i)) != -1]
next_pos = min(next_positions) if next_positions else -1
if next_pos == -1:
nal = data[i:]
i = length
else:
nal = data[i:next_pos]
sc_len = 3 if data[next_pos:next_pos + 3] == b"\x00\x00\x01" else 4
i = next_pos + sc_len
if not nal:
continue
nal_header = nal[0]
if hevc:
nal_type = (nal_header >> 1) & 0x3F
if nal_type in (33, 34):
return True
else:
nal_type = nal_header & 0x1F
if nal_type in (7, 8):
return True
return False
def _decoder_image_batch(output_path: str, topic: str, messages: list[tuple[int, bytes, str]], image_extension: str = ".jpeg") -> None:
if not messages:
return
out_dir = os.path.join(output_path, _sanitize_topic(topic))
os.makedirs(out_dir, exist_ok=True)
messages.sort(key=lambda msg: msg[0])
codec_name = messages[0][2]
if codec_name == "jpeg":
for timestamp_ms, raw_bytes, _ in messages:
timestamp_str = f"{timestamp_ms:013d}"
output_file = os.path.join(out_dir, f"{timestamp_str}{image_extension}")
with open(output_file, "wb") as f:
f.write(raw_bytes)
return
if codec_name == "h265":
codec_name = "hevc"
codec = av.CodecContext.create(codec_name, "r")
seen_keyframe = False
has_extradata = bool(codec.extradata)
is_hevc = codec_name == "hevc"
for timestamp_ms, raw_bytes, _ in messages:
if not has_extradata and not _has_sps_pps(raw_bytes, is_hevc):
continue
packet = av.Packet(raw_bytes)
try:
frames = codec.decode(packet)
except av.error.InvalidDataError:
continue
for frame in frames:
if not has_extradata:
has_extradata = True
if not seen_keyframe:
if not frame.key_frame:
print("not key frame")
continue
seen_keyframe = True
timestamp_str = f"{timestamp_ms:013d}"
output_file = os.path.join(out_dir, f"{timestamp_str}{image_extension}")
cv2.imwrite(output_file, frame.to_ndarray(format="bgr24"))
class BaseCompressedImageDecoder:
target_schema_names: Sequence[str] = ()
skip_schema_encodings: Sequence[str] = ()
image_extension: str = ".jpeg"
def __init__(self) -> None:
self._decoders: dict[int, Callable[[bytes], Any]] = {}
self.decoder_factory = self._build_decoder_factory()
def _build_decoder_factory(self):
raise NotImplementedError
def decoded_message(self, schema: Optional[Schema], channel: Channel, message: Message) -> Any:
decoder = self._decoders.get(message.channel_id)
if decoder is None:
decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
if decoder is None:
raise DecoderNotFoundError(
f"no decoder factory supplied for message encoding {channel.message_encoding}, schema {schema}"
)
self._decoders[message.channel_id] = decoder
return decoder(message.data)
def should_process_schema(self, schema: Schema) -> bool:
if schema.encoding in self.skip_schema_encodings:
return False
return schema.name in self.target_schema_names
def extract_timestamp_ms(self, mcap_msg: Any) -> int:
if hasattr(mcap_msg, "sensor_time") and mcap_msg.sensor_time is not None:
return int(mcap_msg.sensor_time / 1e6)
if hasattr(mcap_msg, "header") and hasattr(mcap_msg.header, "stamp"):
try:
return _stamp_to_ms(mcap_msg.header.stamp)
except AttributeError:
pass
timestamp = getattr(mcap_msg, "timestamp", None)
if timestamp is not None:
try:
return _stamp_to_ms(timestamp)
except AttributeError:
pass
raise AttributeError("message missing timestamp information")
def read_mcap_messages(self, input_bag_path: str, topic_filter: Optional[Sequence[str]] = None) -> dict[str, list[tuple[int, bytes, str]]]:
messages: dict[str, list[tuple[int, bytes, str]]] = {}
with open(input_bag_path, "rb") as f:
reader = make_reader(f, decoder_factories=[self.decoder_factory])
for schema, channel, message in reader.iter_messages():
if not self.should_process_schema(schema):
continue
if not _topic_matches(channel.topic, topic_filter):
continue
mcap_msg = self.decoded_message(schema, channel, message)
if not hasattr(mcap_msg, 'data') or not any(hasattr(mcap_msg, attr) for attr in ('format', 'encode_type')):
continue
try:
timestamp_ms = self.extract_timestamp_ms(mcap_msg)
except AttributeError:
continue
if hasattr(mcap_msg, 'format'):
fmt = str(mcap_msg.format).lower()
else:
fmt = encode_type_map[mcap_msg.encode_type]
messages.setdefault(channel.topic, []).append((int(timestamp_ms), mcap_msg.data, fmt))
return messages
def decode_to_images(
self,
input_bag_path: str,
output_dir: str,
topic_filter: Optional[Sequence[str]] = None,
processes: Optional[int] = None,
) -> None:
messages = self.read_mcap_messages(input_bag_path, topic_filter)
if not messages:
print("no messages matched filter")
return
tasks = [(output_dir, topic, payload, self.image_extension) for topic, payload in messages.items()]
if processes == 1:
for task in tasks:
_decoder_image_batch(*task)
return
with multiprocessing.Pool(processes=processes) as pool:
pool.starmap(_decoder_image_batch, tasks)
pool.close()
pool.join()
class Ros2CompressedImageDecoder(BaseCompressedImageDecoder):
target_schema_names = (
"sensor_msgs/msg/CompressedImage",
"foxglove_msgs/msg/CompressedVideo",
)
skip_schema_encodings = ("ros2idl",)
def _build_decoder_factory(self):
try:
from mcap_ros2.decoder import DecoderFactory # type: ignore
return DecoderFactory()
except ImportError:
raise ImportError("mcap-ros2-support is not installed, please install it with 'pip install mcap-ros2-support'")
class ProtobufCompressedImageDecoder(BaseCompressedImageDecoder):
target_schema_names = (
"foxglove.CompressedImage",
"foxglove.CompressedVideo",
"gwm.sensors.camera.CompressedImage"
)
def _build_decoder_factory(self):
try:
from mcap_protobuf.decoder import DecoderFactory # type: ignore
return DecoderFactory()
except ImportError:
raise ImportError("mcap-protobuf-support is not installed, please install it with 'pip install mcap-protobuf-support'")
# ------------------------------------------------------------------------------------------------
def ros2_main() -> None:
input_bag_path = r"D:\dataset\share\20251028_214041_qa_all_debug_70.mcap"
output_dir = r"D:\dataset\test\output"
topic_filter = ["around/rear", "around/front", "around/right", "around/left"]
decoder = Ros2CompressedImageDecoder()
decoder.decode_to_images(input_bag_path, output_dir, topic_filter)
def protobuf_main() -> None:
input_bag_path = "/home/TECH/gw00365020/workspace/adc_dev/cyber_hmi_dev/gwm_record_20260202_121455.00000.20260202041455.mcap"
output_dir = "./output"
topic_filter = ["rear", "front/fov30", "ront/fov120"]
decoder = ProtobufCompressedImageDecoder()
decoder.decode_to_images(input_bag_path, output_dir, topic_filter)
if __name__ == "__main__":
# ros2_main()
protobuf_main()